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I.  INTRODUCTION 


The  study  of  robotics  is  a  fairly  new  area  in  the  fields  of  science 
and  engineering  which  has  fundamentals  that  can  be  traced  back  to 
many  basic  disciplines,  such  as  electrical  engineering,  mechanical 
engineering,  and  computer  science.  The  mechanical  engineering 
interest  in  the  study  of  robotic  manipulators  can  be  traced  back  to  as 
early  as  the  1940s,  although  the  first  industrial  robots  were  not  intro¬ 
duced  until  the  late  1950s  and  early  1960s  [Ref.  1],  Mechanical  engi¬ 
neers  were  mostly  interested  in  two  areas  of  robotics:  the  control  of 
the  linkage  and  the  study  of  its  motion.  While  these  two  areas  are 
interrelated,  this  thesis  focuses  on  the  second:  the  study  of  the  motion 
of  the  linkage. 

The  study  of  robot  motion  (dynamics)  is  further  divided  into  the 
study  of  arm  dynamics  and  robot  arm  kinematics.  Both  these  divisions 
are  subdivided  into  a  direct  and  an  inverse  problem.  The  direct 
dynamics  problem  is  the  calculation  of  link  state  variables  such  as 
acceleration,  velocity,  and  joint  angles  from  the  known  joint  torques. 
The  inverse  dynamic  problem  assumes  state  variables  are  known  and 
the  joint  torques  are  to  be  calculated.  In  the  kinematics,  the  direct 
problem  is  the  determination  of  the  end  effect  or  position  and  its 
orientation  from  a  given  set  of  link  geometries  and  joint  angles.  The 
inverse  problem  is  the  calculation  of  the  link  geometries  and  joint 
angles  for  a  given  end  effector  position. 
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Various  methods  to  solve  the  inverse  kinematic  problem  use 
matrix  transformations,  inversion,  and  multiplication,  and  as  such  are 
subjected  to  singularity  problems  [Ref.  2).  Singularity  occurs  when  two 
successive  links  are  aligned  (i.e..  the  angle  between  the  links  is  0  or 
180  degrees).  When  a  singularity  occurs,  the  matrices  involved  are  also 
singular  and  not  invertible. 

A  method  to  avoid  the  singularity  problem  was  developed  at  the 
Naval  Postgraduate  School  using  Newtonian  dynamics  in  terms  of  a 
globally  fixed  coordinate  system  [Ref.  3).  This  method  treats  each  link 
as  a  free  body  with  the  forces  and  moments  applied  at  the  joints  and 
the  use  of  Newton’s  law  to  derive  the  equations  of  motion.  In  this 
thesis,  the  development  of  the  free  body  analysis  approach  to  rigid 
manipulators  dynamics  is  continued  and  extended  to  three  dimen¬ 
sions  and  including  gravitational  effects. 
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II.  PROBLEM  STATEMENT 


Theoretical  dynamic  approaches  are  well  developed  and  have 
been  extensively  used  with  simulations.  However,  in  manipulator 
studies,  these  accepted  methods  all  have  a  singularity  that  arises  when 
two  successive  links  are  aligned.  This  does  not  allow  the  motion  to  be 
simulated  (Ref.  3].  This  shortfall  cannot  be  tolerated  in  the  modelling 
of  an  actual  robot  or  robotic  manipulator  because  it  is  of  the  utmost 
importance  to  accurately  know  the  arm  position  at  all  times  (for  real¬ 
time  control  purposes)  (Ref.  2j.  Therefore,  the  problem  directed  by 
this  thesis  research  project  was: 

Continue  the  development  of  the  Newton  Euler  free-body  approach 
for  a  three-link  manipulator  extended  to  three-dimensional  motion 
and  including  gravitational  effects.  In  addition,  compare  simulation 
results  with  those  of  an  actual  robotic  arm.  [Ref.  4J 
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III.  BACKGROUND 


A.  THEORY 

The  basic  theory  for  the  Newton  Euler  free-body  analysis  to  over¬ 
come  the  singularity  problems  of  conventional  robot  dynamics  was 
developed  by  Sadrettin  Altinok  [Ref.  3).  The  theory  of  treating  each 
link  of  the  manipulator  as  a  free  body  with  respect  to  a  global  coordi¬ 
nate  system  leads  to  the  following  27  equations  for  a  three-link 
manipulator. 


FXO  +  M1*AX1  -  FX1  =0  ( 1 ) 

FYO  +  M1*AY1  -  FY1  =  0  (2) 

FZO  +  M1*AZ1  -  FZ1  =  -WG1  (3) 

AX1  +  RB1Z*WD1Y  -  RB  1X*WD1Z  =  AXO  -  MICO  (4) 

where  MICO  =  (X  component  of) 

WD1  X  RBG1  +  W1  X  fWl  X  RBG1)  (4a) 

AY1  -  RB1Z*WD1X  +  RB1X*WD1Z  =  AYO  -  MJCO  (5) 

where  MJCO  =  (Y  component  of  equation  4a). 
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AZ1  +  RBYinVDIX  -  RB1Z*WD1Y  =  AZO  -  MKCO  (6) 

where  MKCO  =  (Z  component  of  equation  4a). 

RB1Z*FY0  -  RB 1  Y*FZO  -  IXXT1*WD1X  +  IXYT1*WD1Y  + 
IXZT1*WD1Z  -  RA1Z*FY1  +  RAIY'FZI  =  T1X  -  TOX  (7) 

-RB1Z*FX0  +  RB1X*FZ0  +  IXYT1*WD1X  -  IYYT1*WD1Y  + 

IYZT1*WD1Z  +  RA1Z*FX1  -  RA1X*FZ1  =  T1Y  -  TOY  (8) 

RB1Y*FX0  -  RB1Z*FY0  +  IXZT1*WD1X  +  IYZT1*WD1Y  - 
IZZT1*WD1Z  -  RA1Y*FX1  +  RA1X*FY1  =  T1Z  -  TOZ  (9) 

FX1  +  M2*AX2  -  FX2  =  0  (10) 

FY1  +  M2*AY2  -  FY2  =  0  (11) 

FZ1  +  M2*AZ2  -  FZ2  =  -WG2  (12) 

-AX1  -  RA1Z*WD1Y  +  RA1Y*WD1Z  +  AX2  +  RB2Z*WD2Y  - 

RB2Y*WD2Z  =  MIC  1  -  MIC2  (13) 

where  MIC1  =  (X  component  of) 

WD1  X  RA1  +  W1  X  fWl  X  RA1)  (13a) 
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where  MIC2  +  (X  component  of) 

WD2  X  RB2  +  W2  X  (W2  X  RB2)  (13b) 

-AY1  +  RA1Z*WD1X  -  RA1X*WD1Z  +  AY2  -  RB2Z*WD2X  + 

RB2X*WD2Z  =  MJC1  -  MJC2  (14) 

where  MJC1  =  (Y  component  of  equation  13a)  and  where  MJC2  =  (Y 
component  of  equation  13b). 

-AZ1  -  RA1Y*WD1X  +  RA1X*WD1Y  +  AZ2  +  RB2Y*WD2X  - 

RB2X*WD2Y  =  MKC 1  -  MKC2  (15) 

where  MKC1  =  (Z  component  of  equation  13a)  and  where  MKC2  =  (Z 
component  of  equation  13b). 

RB2Z*FY1  -  RB2Y*FZ1  -  IXXT2*WD2X  +  IXYT2*WD2Y  + 

IXZT2*WD2Z  -  RA2Z*FY2  +  RA2Y*FZ2  =  T2X  -  T1X  (16) 

-RB2Z*FX1  +  RB2X*FZ1  +  IXYT2*\VD2X  -  IYYT2*WD2Y  + 

IYZT2*WD2Z  +  RA2Z»FX2  -  RA2X*FZ2  =  T2Y  -  T1Y  (17) 

RB2Y*FX  1  +  RB2X*FY1  +  IXZT2*WD2X  +  IYZT2*WD2Y  - 

IZZT2*WD2Z  -  RA2Y*FX2  +  RA2X*FY2  =  T2Z  -  T1Z  (18) 
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FX2  +  M3*AX3  =  0 


(19) 


FY2  +  M3*AY3  =  0  (20) 

FZ2  +  M3*AZ3  =  -WG3  (21) 

-AX2  -  RA2Z*WD2Y  +  RA2Y*WD2Z  +  AX3  +  RB3Z*WD3Y  - 

RB3Y*WD3Z  =  MIC3  -  MIC4  (22) 

where  MIC3  =  (X  component  of) 

WD2  X  RA2  +  W2  X  fW2  X  RA2)  (22a) 

where  MIC4  =  (X  component  of) 

WD3  X  RB3  +  W3  X  fW3  X  RB3)  (22b) 

-AY2  +  RA2Z*WD2X  -  RA2X*WD2Z  +  AY3  -  RB3Z*WD3X  + 

RB3X*WD3Z  =  MJC3  -  MJC4  (23) 

where  MJC3  =  (Y  component  of  equation  22a)  and  where  MJC4  =  (Y 
component  of  equation  22b). 

-AZ2  -  RA2Y*WD2X  +  RA2X*WD2Z  +  AZ3  +  RB3Y*WD3X  - 

RB3X*WD3Y  =  MKC3  -  MKC4  (24) 
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where  MKC3  =  (Z  component  of  equation  22a)  and  where  MKC4  =  (Z 
component  of  equation  22b). 


RB3Z*FY2  -  RB3Y*FZ2  -  IXXT3*WD3X  +  IXYT3*WD3Y  + 

IXZT3*WD3Z  =  -  T2X  (25) 

-RB3Z*FX2  +  RB3X*FZ2  +  IXYT3*WD3X  -  IYYT3*WD3Y  + 

IYZT3*WD3Z  =  -  T2Y  (26) 

RB3Y*FX2  -  RB3X*FY2  +  IXZT3*WD3X  +  IYZT3*WD3Y  - 

IZZT3*WD3Z  =  -  T2Z  (27) 


B.  COMPUTATION 

The  Dynamic  Simulation  Language  (DSL)  was  used  to  simulate  the 
direct  dynamics  problem.  A  matrix  (MATA,  a  27  x  27  matrix)  was  cre¬ 
ated  from  the  known  coefficients  of  the  unknown  variables  in  the  pre¬ 
vious  27  equations.  This  was  possible  due  to  the  assumption  that  the 
change  in  the  inertia  of  the  links,  the  link  velocities,  and  the  joint 
position  must  be  extremely  small  during  a  simulated  step  interval  and 
can  thus  be  treated  as  being  constant  during  the  step.  In  the  direct 
dynamics  problem,  a  vector  (MatB,  27  x  1)  was  generated  from  the 
following  components:  joint  torques,  the  centers  of  gravity  of  the  link, 
and  the  calculated  cross-products  of  angular  velocities  (Figure  1).  This 
gave  a  system  of  equations  in  the  form  of  equation  28: 
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Figure  1.  Matrix  Entries  for  Simulation 
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(28) 


A  *  x  =  b 

where  A  and  b  are  known  and  x  is  an  unknown  vector  (which  contains 
the  forces  at  the  link  ends  and  the  angular  and  linear  accelerations  of 
each  link).  The  1MSL  routine  LEQT2F  (a  linear  equation  solver)  was 
used  to  obtained  the  vector  x.  [Ref.  3  and  Ref.  5] 

In  the  inverse  problem.  MATA  was  developed  the  same  as  in  the 
direct  problem  as  discussed  above,  but  now  vector  x  was  known  (from 
the  history  of  position  data)  and  vector  b  was  found  by  straight  matrix 
multiplication  of  equation  28.  [Ref.  3  and  Ref.  51 

The  previous  work  used  transformation  matrix  operations  to 
obtain  joint  orientations  [Ref.  3].  This  was  done  because  joint  orienta¬ 
tion  cannot  be  obtained  directly  from  the  integration  of  rotational 
velocity  [Ref.  6  and  Ref.  7].  The  approach  used  in  this  thesis  was  one  of 
simple  geometric  relationships.  It  was  reasoned  that  the  linear  accel¬ 
erations  were  either  known  or  calculated  from  the  matrix  operation  of 
LEQT2F  (equation  28).  These  linear  accelerations  were  the  linear 
accelerations  of  the  link  center  of  gravities  with  respect  to  the  earth 
fixed  (inertial)  coordinate  system  which  were  integrated  to  obtain  the 
linear  velocities.  The  velocities  were  further  integrated  to  obtain  the 
position  of  the  center  of  gravities. 

Knowing  the  earth  fixed  coordinates  of  the  centers  of  gravity,  and 
that  each  link  can  be  represented  by  a  rigid  body  [Ref.  8],  it  was 
known  that  the  center  of  gravity  must  be  on  the  vector  from  the 
inboard  joint  to  the  end  of  the  link,  or  next  joint  (Figure  2).  Once  the 
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vector  was  known  to  the  center  of  gravity,  the  directional  cosines 
were  calculated  from  the  following  three  equations  for  each  link. 


DRCOSX  =  [LCOGX  -  JX(inboard)]/LNCOG  (29) 

DRCOSY  =  [LCOGY  -  JY(inboard)]/LNCOG  (30) 

DRCOSZ  =  ILCOGZ  -  JZ(inboard)]/LNCOG  (3 1 ) 

where  LCOGX  is  the  X  location  of  the  CG  with  respect  to  the  inertia 
frame;  JX  is  the  X  location  of  the  inboard  joint  location  with  respect  to 
the  inertia;  and  LNCOG  is  magnitude  of  the  length  from  the  inboard 
joint  to  the  CG. 

Once  the  directional  cosines  were  known,  the  same  method  as 
used  by  previous  investigators  was  used  to  calculate  the  MATA  and 
MATB  matrices. 

C.  CONSTRAINTS 

With  the  desire  to  simulate  the  motion  of  an  actual  robotic  manip¬ 
ulator  and  to  compare  the  simulation  with  data  from  the  manipulator, 
constraints  had  to  be  added  into  the  simulation  so  the  simulation 
would  take  into  account  the  the  joint  mechanical  restrictions.  In  the¬ 
ory,  each  joint  is  a  one  degree  of  freedom  connection.  However,  in  the 
chosen  arm,  the  zero  joint  rotates  in  the  Z  direction  only,  while  joints 
one  and  two  rotate  about  both  the  X  and  Y  axes  (Figure  3).  The  previ¬ 
ously  mentioned  constraints  led  to  the  following  equations: 
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Figure  3.  Robot  Axis  Rotation  with  Constraints 


AX1  =  0 


(32) 


AY1  =0  (33) 

AZ1  =  0  (34) 

WDX1  =  0  (35) 

WDY1  =  0  (36) 

WDZ1  =  WDZ2  =  WDZ3  (37) 


The  concept  for  entering  the  constraints  into  the  simulation  was 
developed  in  Reference  3  and  led  to  the  constraint  section  of  the  DSL 
code  as  seen  in  Appendix  A  and  Appendix  B.  The  basic  concept  which 
was  developed  was  to  zero  out  the  row  only  in  MATA,  then  set  the 
diagonal  of  that  row  to  one  and  the  corresponding  row  of  MATB  to 
zero.  This  concept  was  changed  to  include  zeroing  out  the  row  and 
column  of  MATA  corresponding  to  the  row  of  vector  x,  which  was  to 
be  zero,  along  with  setting  the  diagonal  of  the  row  to  1  and  the  corre¬ 
sponding  row  of  MATB  to  zero. 

D.  GRAVITATIONAL  TORgUES 

The  simulation  of  gravitational  effects  on  the  arm  required  that  an 
equalizing  torque  be  applied  at  the  joints  to  overcome  the  gravitational 
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D.  GRAVITATIONAL  TORQUES 

The  simulation  of  gravitational  effects  on  the  arm  required  that  an 
equalizing  torque  be  applied  at  the  joints  to  overcome  the  gravitational 
pull  (Figure  4).  This  torque  varied  as  the  Z  location  of  the  center  of 
gravity  of  links  2  and  3  varied  because  the  magnitude  of  the  moment 
arm  varied  as  the  link  moved.  The  lever  arm  which  affected  the  gravi¬ 
tational  torque  was  the  projection  of  the  vector  from  the  inboard  joint 
to  the  link  center  of  gravity  onto  the  XY  plane  (Figure  5).  Knowing  this 
lever  arm  and  the  weight  of  the  links,  the  following  equations  were 
used  to  calculate  the  equilibrium  torque  needed  to  overcome  gravity: 

MARM2A  =  SQRT  (LCOGX2*LCOGX2  +  LCOGY2*LCOGY2)  (38) 

MARM2B  =  SQRT  (LCOGX3*LCOGX3  +  LCOGY3*LCOGY3)  (39) 

MARM3  =  SQRT  ((LCOGX3-UX2)**2  +  (LCOGY3-JY2)**2)  (40) 

TGI  =  MAR2A  *  WG2  +  MARM2B  *  WG3  (41) 

TG2  =  MARM3  *  WG3  (42) 

The  equations  above  represent  the  magnitude  of  the  required 
torque  which  must  be  resolved  into  its  X  and  Y  components  (Figure  6). 
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With  the  given  constraints,  this  was  done  in  the  following  manner. 
Angle  THZ  was  calculated  from  the  second  integral  of  WDZ1  (the  base 
link  only  revolved  around  its  vertical  axis).  Then  its  cosine  and  sine 
were  used  to  obtain  the  X  and  Y  components  of  the  gravitational  equi¬ 
librium  torque  as  follows: 

TGX  =  TG  *  COS  (THZ)  (43) 

TGY  =  TG  *  SIN  (THZ)  (44) 


E.  INERTIAS 

The  inertias  of  the  links  of  the  robot  were  calculated  the  first 
time  from  the  equations  of  Reference  2  in  the  global  (inertial) 
reference  frame  and  then  transferred  to  the  local  coordinates  (Figure 
7)  by  the  use  of  a  transformation  [Ref.  9],  giving  the  following  equation: 

Inertia(local)  =  TMAT  *  Inertia(global)  *  TMATTR  (45) 

where  MATA  is  a  transformation  matrix  based  on  the  link  angles 
[Ref.  1  and  Ref.  10].  Knowing  that  the  local  inertias  remain  constant, 
the  global  inertias  for  each  link  were  calculated  for  each  time  iteration 
by  the  use  of  the  inverse  transformation  below: 

Inertia(global)  =  TMATTR  *  Inertia(local)  *  TMAT  (46) 
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TV.  SIMULATION  NONSINGULAR  MOTION  VALIDATION 

A.  TWO-DIMENSIONAL  MOTION  VALIDATION 

A  known  motion  which  had  adjacent  links  aligned  was  that  of  the 
two-dimensional  double  pendulum  (Figure  8).  A  comparison  of  the 
simulation  to  the  theoretical  solution  was  used  to  validate  the 
nonsingularity,  the  motion  tracking,  and  the  gravitational  effects  of  the 
simulation. 

The  theoretical  solution  [Ref.  11]  used  a  Lagrange  equation  which 
only  included  the  linear  kinetic  energy  of  the  system  (equation  47), 
while  the  simulation  included  both  the  linear  and  rotational  kinetic 
energies  (equation  48).  To  account  for  the  difference,  the  inertia  in 
equation  48  was  set  to  a  small  value.  Note  that  it  was  not  possible  to 
set  the  inertias  to  zero  because  the  inertia  entries  in  matrix  MATA 
were  on  the  diagonal  and  would  thus  not  have  allowed  the  inverse  to 
be  calculated. 


Kinetic  Energy  =  1/2  *  m  *  v2  (47) 

Kinetic  Energy  =  1/2  *  m  *  v2  +  1/2  I  *  w2  (48) 

The  point  mass  method  developed  in  Reference  3  allowed  the 
first  mass  of  the  double  pendulum  to  be  set  to  a  value  equal  to  that  of 
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Figure  8.  Double  Pendulum  Configuration 


the  outer  point  mass  of  link  2  plus  the  inner  point  mass  of  link  3 
(equation  49).  The  second  mass  of  the  double  pendulum  was  set  to  the 
value  of  the  outer  point  mass  of  link  3  (equation  50). 


ml  (dp)  =  m  (2,2)  +  m  (3.1) 

(49) 

m2  (dp)  =  m  (3.2) 

(50) 

The  lengths  of  the  double  pendulum  were  the  same  as  the  link 
lengths  (Figure  8). 

The  results  of  the  theoretical  and  simulation  program  for  the 
double  pendulum  problem  agreed  (Figures  9  and  10).  and  would  have 
been  identical  had  the  inertia  been  set  to  zero.  Tracking  of  the  motion 
through  the  potential  singular  points  presented  no  difficulties  to  the 
simulation  (Figure  11). 

B.  THREE-DIMENSIONAL  NONSINGULAR  MOTION 

A  motion  which  had  potential  singular  points  was  chosen  which 
validated  the  nonsingularity  of  the  simulation  in  three  dimensions 
(Figure  12).  The  motion  which  was  chosen  was  to  input  the  joint  1  and 
joint  2  angles  as  a  time  function  and  allow  the  third  joint  angle  to 
remain  zero,  so  links  2  and  3  were  always  aligned  and  potentially 
singular.  This  chosen  motion  also  had  all  three  links  aligned  at  time 
equal  to  0.785  seconds  (Figure  12). 
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Figure  9.  Double  Pendulum  Angle  2 
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Figure  10.  Double  Pendulum  Angle  3 
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Figure  1 1 .  Double  Pendulum  Simulation  Through 
Potential  Singular  Points  (angle  *  0) 
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The  simulation  again  had  no  difficulties  tracking  the  input  motion 
through  the  potentially  singular  points  (Figure  13).  The  error  in  the 
predicted  and  simulated  end  effector  position  (Figure  14)  was  never 
greater  than  1.0  percent. 
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Figure  14.  End  Effector  Position  Error 
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V.  EXPERIMENTAL  VALIDATION 


A.  MANIPULATOR 

A  robot  manipulator  was  chosen  to  check  the  agreement  of  the 
simulation  to  data  from  an  actual  robot  arm.  The  arm  that  was  available 
was  the  NEPTUNE  II  (Figure  15),  which  is  a  low-performance, 
hydraulic-operated  arm.  This  arm  required  certain  hardware  modifi¬ 
cations,  namely: 

•  the  remounting  of  the  electronic  solenoid  boards  to  facilitate 
access  and  provide  protection  from  hydraulic  leaks; 

•  installation  of  bleed  holes  in  the  major  joint  pistons  to  allow  air  to 
be  bled  off  when  the  system  has  been  opened  for  repairs; 

•  design  and  manufacture  of  a  pump  and  accumulator  stand  that 
would  provide  spray  protection  for  the  manipulator  in  the  case  of 
malfunctions; 

•  modification  to  the  plumbing  of  the  pump  and  accumulator  to 
include  an  electrically  controlled  solenoid  valve  operated  from  the 
Electronic  Control  Panel  (ECP); 

•  rewiring  the  pump  to  be  electrically  controlled  from  the  ECP; 

•  redesign  and  manufacture  of  the  waist  joint  (joint  0)  shaft  and 
coupling,  along  with  that  of  several  piston  end  plates;  and 

•  an  overhaul  of  the  complete  electronics. 

B.  PRESSURE  TRANSDUCERS 

Pressure  transducers  which  had  previously  been  installed  on  the 
NEPTUNE  II  arm  at  joints  1  and  2  (Figure  16)  allowed  for  data  collec¬ 
tion  to  validate  the  two-dimensional  simulation  with  gravitational 
effects.  Initially,  the  pressure  transducers  had  to  be  calibrated. 
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Figure  15.  NEPTUNE  II  Robot 
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Joint  2 

Figure  16.  Robot  Pressure  Transducers 


33 


A  3,000-pound  nitrogen  bottle  with  a  200  psi  regulator  was  used 
for  pressure  transducer  calibration.  This  was  connected  to  a  test  rig 
which  consisted  of  a  varlinear  pressure  adjustment  and  a  200-pound 
pressure  gage  which  had  been  calibrated  and  was  incremented  in  0.2- 
pound  increments  (Figure  17).  The  varlinear  allowed  the  pressure  at 
the  transducer  to  be  varied  in  fine  increments  from  zero  to  120  psi, 
which  was  the  stated  range  of  the  transducers.  The  electrical  connec¬ 
tions  from  the  transducers  had  been  hard-wired  to  the  ECP  and  then 
to  a  digital  multimeter  (Fluke  8024  B):  the  meter  20-volt  scale  was 
used  to  read  the  voltage  output  of  the  transducers  (the  voltage  was 
read  to  the  second  decimal  place).  Each  Omega  Series  PX302  pressure 
transducer  had  two  complete  sets  of  data  from  0  to  120  psi;  the  aver¬ 
ages  of  these  are  provided  in  Table  1.  The  pressure  transducers  were 
very  linear  from  0  to  115  psi,  with  each  5  psi  producing  a  voltage  of 
0.5  volts  (Figure  18).  This  range  of  pressures  was  well  acceptable  for 
the  NEPTUNE  II,  which  operates  between  0  and  118  psi  (Ref.  12). 
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Figure  17.  Pressure  Transducer  Calibration  Set-Up 


TABLE  1 


PRESSURE  TRANSDUCER  DATA 


Pressure 

PI 

P2 

P3 

P4 

(psi) 

volts 

volts 

volts 

volts 

000 

-0.010 

0.000 

0.015 

0.000 

005 

0.500 

0.510 

0.520 

0.500 

010 

0.995 

1.005 

1.025 

1.005 

015 

1.500 

1.500 

1.530 

1.500 

020 

1.985 

1.995 

2.020 

2.000 

025 

2.500 

2.500 

2.530 

2.500 

030 

3.000 

3.000 

3.030 

3.000 

035 

3.490 

3.500 

3.530 

3.490 

040 

3.990 

3.995 

4.030 

3.995 

045 

4.500 

4.500 

4.530 

4.500 

050 

4.995 

4.995 

5.025 

4.990 

055 

5.490 

5.500 

5.520 

5.490 

060 

5.995 

5.995 

6.020 

5.990 

065 

6.490 

6.500 

6.520 

6.490 

070 

6.990 

7.000 

7.020 

6.990 

075 

7.490 

7.500 

7.510 

7.500 

080 

7.990 

8.005 

8.010 

8.000 

085 

8.490 

8.510 

8.510 

8.500 

090 

8.990 

9.005 

9.000 

9.000 

095 

9.480 

9.500 

9.500 

9.500 

100 

9.990 

10.005 

10.000 

10.000 

105 

10.490 

10.510 

10.500 

10.500 

110 

10.990 

11.010 

11.000 

11.000 

115 

11.485 

11.490 

11.490 

11.500 
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Figure  18.  Pressure  Transducer  Data 
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C.  MEASUREMENTS 
1.  Link  Lengths 

With  the  necessity  to  disassemble  the  NEPTUNE  II  for 
repairs,  the  link  length  measurements  were  physically  measured. 
These  measurements  are  provided  in  Table  2  to  the  nearest  1/8  inch. 

TABLE  2 

LINK  LENGTHS 


Parameter 

Length 

Length 

inch 

meter 

LINKL1 

10.250 

0.2604 

LINKL2 

16.375 

0.4159 

LINKL3 

13.750 

0.3498 

2.  Link  Masses 

Also  during  the  disassembly  of  the  robot,  the  link  masses 
were  measured  using  a  scale  which  was  in  half-pound  increments.  The 
design  of  the  robot  allowed  for  a  good  approximation  of  the  two 
masses  per  link  as  used  in  the  simulation  (Figures  19  and  20).  This 
was  a  crude  measurement,  but  it  was  felt  that  this  was  sufficient  to  be 
used  for  the  simulation  and  that  the  time  necessary  to  completely  dis¬ 
assemble  the  robot  in  order  to  be  able  to  obtain  closer  measurements 
was  not  justified.  The  masses  which  were  measured  are  provided  in 
Table  3. 
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Figure  19.  Robot  Link  Masses 
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TABLE  3 


LINK 

MASSES 

Parameter 

Pounds 

Kilograms 

MASS(1,1)  est. 

5 

2.273 

MASS(1,2)  est. 

15 

6.818 

Ml  (measured) 

20 

9.091 

MASS(2, 1)  est. 

1 

0.455 

MASS(2,2)  est. 

1 

0.455 

M2  (measured) 

2 

0.910 

MASS(3,1)  est. 

13 

5.909 

MASS(3,2)  est. 

13 

5.909 

M3  (measured) 

26 

11.818 

3.  Length  from  COG  of  Link  to  Center  of  Mass 

The  point  mass  centers  were  assumed  to  be  at  the  joints 
(Figure  20).  A  simple  calculation  of  the  moments  around  the  center  of 
gravity  was  performed;  this  provided  the  data  in  Table  4. 

TABLE  4 

LENGTH  FROM  LINK  CENTER  OF  GRAVITY 
TO  MASS  CENTER  OF  GRAVITY 


Parameter 

Length 

inch 

Length 

meter 

L(l.l) 

7.6875 

0.1953 

L(  1.2) 

2.5625 

0.0651 

L(2, 1) 

8.1875 

0.2080 

L(2,2) 

8.1875 

0.2080 

L(3, 1) 

6.8750 

0.1746 

L(3,2) 

6.8750 

0.1746 

D.  DATA  COLLECTION 

Position  data  and  pressure  data  were  taken  for  several  movements 
of  links  2  and  3  of  the  robot  from  the  electronic  control  panel  and 
recorded  on  a  four-channel  strip  chart  recorder.  An  example  of  the 
recorded  data  is  provided  (Figure  21). 

E.  DATA  REDUCTION 

The  data  taken  for  the  various  runs  was  reduced  using  the  follow¬ 
ing  equation  to  obtain  joint  torques. 

TORQUE  =  A  PRESSURE  *  AREA(piston)  *  RADIUS(lever  arm)  (51) 

where  AREA(piston)  =  0.00312  square  meters  and  RADIUS(lever  arm) 
=  0.0597/2  meters. 

F.  VALIDATION 

The  delta  pressure  data  versus  time  was  entered  into  the  robot 
validation  program  as  a  time  function  and  the  torque  was  calculated  as 
a  function  of  time  from  this  data.  The  simulation  position  results 
agreed  with  the  actual  robot  position  in  the  general  trend  of  motion 
(Figure  22).  It  was  felt  that  the  inaccuracies  in  the  results  were  due  to 
both  the  simulation  round-off  errors  and  the  crude  methods  used  in 
the  collection  of  the  robot  link  masses  and  link  lengths,  particularly 
the  lengths  from  the  center  of  gravity  of  the  link  to  the  center  of 
gravity  of  the  point  masses.  The  scalloped  shape  of  the  simulated 
result  was  attributed  to  the  normalization  of  the  directional  cosines. 


DIRECTIONAL  COSINE  ANGLE  LINK  3 
100  120  110  160  180 


VI.  RESULTS  AND  RECOMMENDATIONS 


1.  A  nonsingular  dynamic  simulation  of  a  rigid  revolute  three-link 

manipulator  has  been  further  developed.  It  includes: 

•  Gravity, 

•  Three-Dimensional  Motion, 

•  Comparatively  straightforward  dynamics. 

2.  Accuracy  and  nonsingularity  has  been  validated  with  a  double 

pendulum. 

3.  Three-dimensional  nonsingularity  has  been  validated. 

4.  Procedures  for  actual  robot  data  collection  have  been  developed. 

5.  The  following  recommendations  are  provided: 

•  Simulate  actual  three-dimensional  robot  performance  and 
compare  predicted  to  measured  variables. 

•  Enhance  the  computer  program  code  to  enable  it  to  be  more 
interactive.  That  is,  let  the  user  specify  the  constraints  during 
execution. 

•  Use  this  simulation  to  develop  advanced  controllers  for  rigid 
robot  linkages. 
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APPENDIX  A 


DIRECT  DYNAMIC  SIMULATION  PROGRAM 


TITLE  DIRECT  DYNAMICS  PROBLEM 


* 

Vr 

* 


THIS  IS  A  PROGRAM  THAT  WILL  SOLVE  THE  DIRECT  DYNAMICS  PROBLEM  FOR  A  * 
3  LINK  RIGID  BODY  MANIPULATOR  WITHOUT  THE  USE  OF  ANY  TRANSFORMATION  * 
MATRICES.  THE  ORIGINAL  PROGRAM  WAS  WRITTEN  BY  LT  ATINOK  AND  HAS  * 
HAS  BEEN  GREATLY  MODIFIED  TO  INCLUDE  3  D  MOTION  AND  GRAVITATIONAL  * 
EFFECTS  BY  LT  R.  M.  VERBOS  USN  SEPTEMBER  1988  * 


TERMINAL 
METHOD  RKSFX 

PRINT  . 10 , DRCANX( 1 ) , DRCANY( 1 ) , DRCANZ( 1 ) , .  .  . 

DRCANXC 2) , DRCANY ( 2 ) , DRC ANZ ( 2 ) , DRCANX( 3 ) , DRCANY ( 3 ) ,DRCANZ(3) .  . 

DRCSX( 1) ,DRCSY( 1) ,DRCSZ( 1) ,DRCSX(2) ,DRCSY(2) ,DRCSZ(2) ,DRCSX(3) .  . 

DRCSY( 3) ,DRCSZ( 3 ) , .  .  . 

JXO , JYO , JZO , JX1 , JY1 , JZ1 , JX2 , JY2 , J22 , .  .  . 

LCCGX(  1 ) , LC0GY(  1 ) , LCOGZ( 1) , LC0GX( 2) , LC0GY( 2 ) , LCOGZC  2 ) , .  .  . 

LC0GXC3) ,LC0GY(3) ,LCOGZ(3) .  . 

TOv  TOY  TOZ  Tiv  T1Y  TtZ  T2X  T2Y  T27. 

WDX(l) ,WDY( 1) ,WDZ( 1) ,WDX(2)!vDY(2) ,vi)2(2) ,WDX(3) ,WDY(3) ,WDZ(3) .  . 

AX1 , AY1 , AZ1 , AX2 , AY2 , AZ2 , AX3 , AY3 ,AZ3 , .  .  . 

W1X.W1Y ,W1Z ,W2X,W2Y,W2Z ,W3X,W3Y ,W3Z .... 

FXO , FYO ,FZ0 ,FX1 ,FY1 ,FZ1 ,FX2 ,FY2 ,FZ2 , .  .  . 

IXXT(  1 ) , IYYT( 1 ) , I ZZT( 1 ) , IXXT( 2 ) , 1YYT( 2) , IZZT( 2) , IXXT( 3) , IYYT( 3 ) , .  .  . 
IZZT(3),IXYT(1),IYZT(1) , IXZT( 1) , IXYT( 2) , IYZT( 2) , IXZT( 2) , IXYT( 3) , .  .  . 

IYZT( 3) , IXZT( 3) , .  .  . 

LNCOG 1 , LNC0G2 , LNCOG 3 , THZANG , COSTHZ , S INTHZ , TI PX , TIPY , TI PZ , .  .  . 

ANG12X, ANG12Y, ANG12Z , ANG23X , ANG23Y , ANG23Z , .  .  . 

T1CH,T2CH,IER 

CONTROL  FINTIM  =1.5,  DELT  =  .0005,  DELMAX  =  .  1,  DELPRT  =  .  10 
D  DIMENSION  MATA(27,27) ,MASS( 3 , 2) ,L( 3 ,2 ) ,RX( 3 , 2) ,RY( 3 , 2) ,RZ( 3 , 2) 

D  DIMENSION  IXX( 3 ,2) , IXZ( 3 ,2) , IXY( 3,2) , IYY( 3 ,2) , IYZ( 3 , 2) , IZZ(  3 , 2) 

D  DIMENSION  IMATC 3 , 3 , 3) ,NIMAT( 3 , 3) ,TMAT( 3 , 3) ,TMATTR( 3 , 3) ,MATTMP( 3 ,3) 
D  DIMENSION  LIMAT( 3,3,3) 

FIXED  IER , I , J,M,K,P,N, IA, IDGT,A, II 

ARRAY  MATB (27), ABMATB ( 2 7 ) , LCOGX( 3 ) , LCOGY( 3 ) , LCOGZ ( 3 ) 

ARRAY  VECTAO<  3 ) , VECTBOC  3 ) , VECTA 1 ( 3 ) , VECTB 1 ( 3 ) , VECTA2C 3 ) , VECTB 2 ( 3 ) 

ARRAY  VECTAC  3 ) , VECTB ( 3 ) 

ARRAY  WDX(3),WDY(3),WDZ(3),W1(3) ,W2(3),W3(3) 

ARRAY  RBG1( 3) ,RAG1( 3) ,RBG2( 3) ,RAG2(3) ,RBG3(3) 

ARRAY  WKAREA( 850) 

ARRAY  IXXT( 3) , IYYT( 3) , IZZT( 3) , IXYT( 3) , IXZT( 3) , IYZT(3) 
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ARRAY  DRCANX(3) ,DRCANY( 3) ,DRCANZ(3) 
ARRAY  DRCSX( 3) ,DRCSY( 3) ,DRCSZ( 3) 

INITIAL 


*****  input  ***** 

*  INPUT  PARAMETER  CONSTANTS 

55  A  =  15. 0D0 

P  =  0.  ODO 
W  =  2  *  PI 

IDGT  =  6 
G  =  9.  8 IDO 
N  =  27 
M  =  1 
IA  =  27 
IER  =  0 
LEVELQ  =  0 
LEVLDQ  =  0 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  0. ODO 
JYO  =  0. ODO 
JZO  =  0. ODO 
JX1  =  0. ODO 
JY1  =  0. ODO 
JZ1  =  0. 2D0 
JX2  =  0.  ODO 
JY2  =  0.416D0 
JZ2  =  0.2D0 

*  INPUT  TORQUE  CONSTANTS 


TOX 

= 

0.  ODO 

TOY 

= 

0.  ODO 

TOZ 

= 

0.  ODO 

T1X 

= 

0.  ODO 

T1Y 

= 

0.  ODO 

T1Z 

= 

0.  ODO 

T2X 

= 

0.  ODO 

T2Y 

= 

0.  ODO 

T2Z 

= 

0.  ODO 

TGI 

= 

0.  ODO 

TG2 

= 

0.  ODO 

T1FNC 

= 

odo 

T2FNC 

= 

0.  ODO 

*  INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS 

*  FOR  EACH  LINK  ENDS 

L( 1 , 1)  =  0. 05D0 
L(  1,2)  =  0. 15D0 
L(2 , 1)  =  0.20D0 
L(  2 ,2)  =  0.216D0 
L(  3 , 1)  =  0. 225D0 
L(3,2)  =  0. 226D0 

*  INPUT  THE  LINK  LENGTHS  OF  THE  ROBOT 

LINKL1  =  0. 20D0 
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LINKL2  =  0.416D0 
LINKL3  =  0.45  IDO 


*  INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 

no  mass(i,i)  =  n.oDO 

MASS( 1 ,2)  =  33. ODO 
MASS( 2,1)  =  2.  2D0 
MASS(2,2)  =  2.  2D0 
MASS( 3,1)  =  28.6D0 
MASS ( 3 ,2)  =  28. 6D0 

*  INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 


LCOGX(l) 

= 

0.  ODO 

XI 

= 

LCOGX(l) 

LCOGY(l) 

= 

0.  ODO 

Y1 

= 

LCOGY(l) 

LCOGZ( 1) 

= 

0.  10D0 

Z1 

LCOGZ(l) 

LC0GXC2) 

= 

0.  ODO 

X2 

= 

LCOGX( 2) 

LC0GYC2) 

= 

0.  208D0 

Y2 

= 

LCOGY( 2) 

LC0GZ(2) 

= 

0.  20D0 

Z2 

= 

LCOGZ( 2 ) 

LC0GX(3) 

= 

0.  ODO 

X3 

= 

LC0GX(3) 

LCOGY( 3) 

= 

0.  6415D0 

Y3 

= 

LCOGY( 3) 

LCOGZ( 3) 

= 

0.  2D0 

Z3 

= 

LCOGZ( 3) 

INPUT  MASS  OF  EACH  LINK  IN  KG 
Ml  =  44. ODO 
M2  =  4.4D0 
M3  =  57.2DO 

INPUT  ACCELERATIONS  OF  JOINT  ZERO 
AXO  =  0. ODO 
AYO  =  0. ODO 
AZO  =  0. ODO 

INPUT  THE  INITIAL  DIRECTION  COSINES 
DRCSX(l)  =  0. ODO 
DRCSY(l)  =  0.  ODO 
DRCSZ(l)  =  1. ODO 
DRCSX( 2)  =  0. ODO 
DRCSY( 2)  =  1.  ODO 
DRCSZC  2)  =  0. ODO 
DRCSX( 3)  =  0. ODO 
DRCSY( 3)  =  1.  ODO 
DRCSZ( 3)  =  0. ODO 

INPUT  THE  INITIAL  DIRECTION  COSINE  ANGLES 
DRCANX(l)  =  90. ODO 
DRCANY( 1 )  =90. ODO 
DRCANZ(l)  =  0. ODO 
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DRCANX(2)  =  90. ODO 
DRCANY( 2)  =  0. ODO 

DRCANZ( 2)  =  90. ODO 
DRCANX( 3)  =  90. ODO 
DRCANY( 3)  =  0.  ODO 

DRCANZ( 3)  =  90. ODO 

*****  INITIALIZE  ***** 

*  OMEGA  AND  OMEGA  DOT 


160 

DO  170  I  =  1 

,3 

W1(I) 

0.  ODO 

W2(  I) 

= 

0.  ODO 

W3(  I ) 

= 

0.  ODO 

WDX(I) 

= 

0.  ODO 

WDY(I) 

= 

0.  ODO 

WDZ(I) 

= 

0.  ODO 

170 

CONTINUE 

THZ  =  0.  010 

*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA( I , J)  =  0. ODO 
175  CONTINUE 

MATB(I)  =  0. ODO 
180  CONTINUE 


*****  CALCULATIONS  ***** 
*  WEIGHTS  (NEWTONS) 

185  WG1  =  M1*G 

WG2  =  M2*G 
WG3  =  M3*G 


COMPUTE  THE  LENGTH 
LNC0G1  =  DSQRT 

LX 2  =  LC0GX(2) 
LY2  =  LCOGY( 2) 
LZ2  =  LCOGZ( 2 ) 
LNC0G2  =  DSQRT 
LX3  =  LCOGX( 3 ) 
LY3  =  LCOGY( 3) 
LZ3  =  LC0GZ(3) 
LNC0G3  =  DSQRT 


FROM  THE  INBOARD  JOINT  TO  COG 


(  LCOGX( 1)*LC0GX( 1)  + 
LC0GZ(1)*LC0GZ<' 1)  ) 

-  JX1 

-  JY1 

-  JZ1 

(  LX2*LX2  +  LY2*LY2  +  LZ2*LZ2  ) 

-  JX2 

-  JY2 
_  J22 

(  LX3*LX3  +  LY3*LY3  +  LZ3*LZ3  ) 


LCOGY( 1)*LC0GY( 1)  +. 


*  COMPUTE  INITIAL  INERTIAS  BASED  ON  POINT  MASSES 

*  IN  GLOBAL  COORDINATES 

190  DO  225  I  =  1,3 

RX( 1,1)  =  -L(I,1)  *  DRCSX(I) 

RX( 1,2)  =  L( I ,2)  *  DRCSX(I) 

RY ( 1 , 1 )  =  -LC 1,1)  *  DRCSY(I) 

RY ( I , 2 )  =  L( I , 2)  *  DRCSY(I) 

RZ( 1,1)  =  - L( 1,1)  *  DRCSZ(I) 

RZ( 1,2)  =  L( I , 2)  *  DRCSZ(I) 

200  IXX(I,1)  =  MASS( 1,1)  *  ( C RY C 1,1)  *  RY(I,1))  +  (RZ(I,1)  *  RZ(I,1))) 
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IXX(I,2)  =  MASS( 1,2)  *  ( (RY( 1,2)  *  RY(I,2))  +  (RZ(I,2)  *  RZ(I,2))) 
IXXT(I)  =  IXX(I,1)  +  IXX( 1,2) 

IYY( 1,1)  =  MASS( 1,1)  *  ( ( RX( 1,1)  *  RX(I,1))  +  (RZ(I,1)  *  RZ(I,1))) 

IYY( 1,2)  =  MASS( 1,2)  *  ( ( RX( 1,2)  *  RX(I,2))  +  (RZ(I,2)  *  RZC 1,2)^^ 

IYYT(I)  =  IYY( 1,1)  +  IYY( 1,2) 

IZZ( 1,1)  =  MASSCI.l)  *  ( (RX( 1,1)  *  RX(I,1))  +  (RY(  1,1)  *  RY(I,1))) 

I ZZ ( 1,2)  =  MASS( 1,2)  *  ( ( RX( 1,2)  *  RX(I,2))  +  (RY(I,2)  *  RY(I,2))) 

IZZT(I)  =  IZZ(I,1)  +  IZZ( 1,2) 

205  IXY (1,1)  =  MASSCI.l)  *  RX(I,1)  *  RY(I,1) 

IXY( 1,2)  =  MASS( 1,2)  *  RX( 1,2)  *  RY(I,2) 

IXYT(I)  =  IXY(I,1)  +  IXY( 1,2) 

IXZ( 1,1)  =  MASSCI.l)  *  RZ(I,1)  *  RX( 1,1) 

IXZ( 1,2)  =  MASSC 1,2)  *  RZ( 1,2)  *  RX(I,2) 

IXZT(I)  =  IXZ(I,1)  +  IXZ( 1,2) 

IYZ(I.l)  =  MASSCI.l)  *  RY(I.l)  *  RZ( 1,1) 

IYZ(I,2)  =  MASSC 1, 2)  *  RYC 1,2)  *  RZ(I,2) 

IYZT(I)  =  IYZ(I,1)  +  IYZCI.2) 

IF  CIXXTCI)  .  LE.  .02)  THEN 

IXXT(I)  =  .02 

ELSE 

IXXTCn  =  IXXT(I) 

END  IF 

IF  CIYYTCI)  .  LE.  .  02)  THEN 
IYYT(I)  =  .02 

FT  c;F 

IYYT(I)  =  IYYT(I) 

END  IF 

IF  CIZZT(I)  . LE.  . 02)  THEN 

IZZTCD  =  .02 

ELSE 

IZZT(I)  =  IZZTCI) 

END  IF 

IMATC I, 1,1)  =  IXXT(I) 

IMATC 1,1,2)  =  IXYTCI) 

IMAT( 1,1,3)  =  IXZTC1) 

IMATC 1,2,1)  =  -IXYTCI) 

IMATC I, 2, 2)  =  IYYTCI) 

IMATC I, 2, 3)  =  IYZTCT) 

IMAT( 1,3,1)  =  -IXZTCI) 

IMATC 1,3,2)  =  -IYZT(l) 

IMAT( 1,3,3)  =  IZZTCI) 

225  CONTINUE 

*  DUE  TO  LINK  1  CONSTRAINTS  LINK  1  INERTIAS  ARE  CONSTANT 

1XaT(1)  =  IMATC 1,1,1) 

IXYTCI)  =  IMATC 1,1,2) 

IXZTCI)  =  IMATC 1,1, 3) 

IYYT(l)  =  IMATC 1,2,2) 

IYZT(l)  =  IMATC 1,2, 3) 

IZZTCI)  =  IMATC 1,3,3) 

*  TRANSFORM  THE  INITIAL  INERTIAS  TO  LOCAL  COORDINATED 

DO  9  I  =  2,  3 

TMATC  2,1)  =  -DCOS  C  THZ  ) 

TMATC  2,2)  =  -DSIN  (  THZ  ) 

TMATC 2, 3)  =  0.  ODO 
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TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT( 3,3)  =  DRCSZ(I) 

VECTA(l)  =  TMAT(2,1) 

VECTA(2)  =  TMAT (2,2) 

VECTA( 3)  =  TMAT( 2,3) 

VECTB(l)  =  TMAT( 3,1) 

VECTB( 2)  =  TMAT( 3,2) 

VECTB( 3)  =  TMAT( 3,3) 

CALL  CPROD  ( VECTA.VECTB ,MI1 ,MJ1 ,MK1) 

TMAT( 1,1)  =  Mil 
TMAT( 1,2)  =  MJ1 
TMAT( 1,3)  =  MK1 
TMATTR( 1,1)  =  TMAT( 1,1) 

TMATTR( 1,2)  =  TMAT(2,1) 

TMATTR( 1,3)  =  TMAT(3,1) 

TMATTR( 2,1)  =  TMAT(1,2) 

TMATTR( 2,2)  =  TMAT(2,2) 

TMATTR( 2,3)  =  TMAT(3,2) 

TMATTR( 3,1)  =  TMAT(1,3) 

TMATTR( 3,2)  =  TMAT(2,3) 

TMATTR( 3,3)  =  TMAT(3,3) 

DO  5  II  =  1,3 
DO  5  J  =1,3 
TEMP  =  0. ODO 
DO  1  K  =1,3 

TEMP  =  TMAT( I 1 ,K)  *  IMAT(I,K,J)  +  TEMP 
1  CONTINUE 

MATTMP( 1 1 , J)  =  TEMP 
5  CONTINUE 

DO  8  II  =  1,3 

DO  8  J  =  1,3 

TEMP  =  0. ODO 
DO  7  K  =  1,3 

TEMP  =  MATTMP( 1 1 ,K)  *  TMATTR(K,J)  +  TEMP 

7  CONTINUE 

LIMAT(I . I 1 , J)  =  TEMP 

8  CONTINUE 

9  CONTINUE 

DERIVATIVE 

NOSORT 

230  CALL  ERRSET  (208,256,-1,1,1) 

CALL  UERSET( LEVELQ , LEVLDQ ) 

*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  240  I  =  1,27 
DO  235  J  =  1,27 

MATA( I , J)  =  0. ODO 
235  CONTINUE 

MATB(I)  =  0. ODO 
240  CONTINUE 


INPUT  JOINT  EQUATIONS 
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*  JOINT  ZERO  EQUATIONS 

*  W1  X  (W1  X  RB/G1) 

RBGl(l)  =  JXO  -  LCOGX(l) 

RBG1(2)  =  JYO  -  LCOGY(l) 

RBG1( 3)  =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl( 2) 

VECTAO)  =  W 1 ( 3 ) 

CALL  CPROD( VECTAO , RBG1 , MICO , MJCO , MKCO) 
VECTBO(l)  =  MICO 
VECTBO( 2)  =  MJCO 
VECTBO( 3)  =  MKCO 

CALL  CPROD( VECTAO , VECTBO , MICO , MJCO , MKCO ) 

*  INPUT  JOINT  EQUATIONS 

*  JOINT  ZERO  EQUATIONS 

*  W1  X  (W1  X  RB/G1 ) 

RBG1(1)  =  JXO  -  LCOGX(l) 

RBG1( 2)  =  JYO  -  LCOGY(l) 

RBG1( 3)  =  JZO  -  LCOGZ(l) 

VECTA( 1)  =  W 1 ( 1 ) 

VECTAO)  =  Wl(2) 

VECTAI 3 )  =  Wl( 3 1 

CALL  CPROD( VECTA , RBG1 , MICO , MJCO , MKCO ) 
VECTBO)  =  MICO 
VECTBO)  =  MJCO 
VECTBO)  =  MKCO 

CALL  CPROD( VECTA, VECTB , MICO , MJCO , MKCO) 

*  VI  X  (VI  X  RA/G1 ) 

RAGl(l)  =  JX1  -  LCOGX(l) 

RAG  1(2)  =  JY1  *  LCOGY(l) 

RAG1( 3)  =  JZ1  -  LCOGZ(l) 

VECTAO)  =  Wl(l) 

VECTA( 2)  =  Wl( 2) 

VECTAO)  =  Wl(3) 

CALL  CPROD  (VECTA, RAG1 ,MIC1 ,MJC1 ,MKC1) 
VECTBO)  =  MIC1 
VECTB ( 2)  =  MJC1 
VECTBO)  =  MKC1 

CALL  CPROD  ( VECTA, VECTB, MI Cl  ,MJC1 ,MKC1 ) 

*  W2  X  ( W2  X  FB/G2) 

RBG2(1)  =  JX1  -  LCOGX( 2 ) 

RBG2( 2)  =  JY1  -  LCOGY( 2 ) 

RBG2( 3)  =  JZ1  -  LCOGZ( 2) 

VECTAO)  =  W2(  1) 

VECTA( 2)  =  W2( 2) 

VECTAO)  =  W2(  3  j 

CALL  CPROD  ( VECTA, RBG2 ,MIC2 ,MJC2 ,MKC2) 
VECTB( 1)  =  MIC2 
VECTBO)  =  MJC2 
VECTBO)  =  MKC2 

CALL  CPROD  (VECTA, VECTB, MIC2 ,MJC2 ,MKC2) 

*  V2  X  (W2  X  RA/G2) 

RAG2( 1 )  =  JX2  -  LCOGX(2) 

RAG2( 2)  =  JY2  -  LCOGY(2) 
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RAG2( 3)  =  JZ2  -  LCOGZ( 2) 

VECTA(l)  =  W2( 1) 

VECTA( 2)  =  V2( 2) 

VECTA( 3)  =  W2( 3) 

CALL  CPROD  (VECTA, RAG2,MIC3,MJC3,MKC3) 
VECTB(l)  =  MIC3 
VECTB( 2)  =  MJC3 
VECTB(3)  =  MKC3 

CALL  CPROD( VECTA, VECTB ,MIC3 ,MJC3 ,MKC3) 
W3  X  (W3  X  RB/G3) 

RBG3( 1)  =  JX2  -  LCOGX( 3) 

RBG3( 2)  =  JY2  -  LCOGY(3) 

RBG3( 31  =  JZ2  -  LCOGZ( 3) 

VECTA(l)  =  W3C  1) 

VECTA( 2)  =  W3C  2 ) 

VECTA( 3)  =  W3C  3) 

CALL  CPROD  (VECTA, RBG3 ,MIC4 ,MJC4 ,MKC4) 
VECTB ( 1 )  =  MIC4 
VECTB(2)  =  MJC4 
VECTB ( 3)  =  MKC4 

CALL  CPROD  (VECTA, VECTB, MIC4.MJC4.MKC4) 

INERTIA  TRANSFORMATION 
DO  390  I  =  2,  3 

TMAT( 2,1)  =  -DCOS  (  THZ  ) 

TMAT( 2,2)  =  -DSIN  (  THZ  ) 

TMAT( 2,3)  =  0.  ODO 
TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT( 3,3)  =  DRCSZ(I) 

VECTA(l)  =  TMATC  2,1) 

VECTA( 2)  =  TMAT( 2,2) 

VECTA( 3)  =  TMAT( 2,3) 

VECTB(l)  =  TMAT( 3,1) 

VECTB ( 2)  =  TMAT( 3,2) 

VECTB( 3)  =  TMAT( 3,3) 

CALL  CPROD  ( VECTA , VECTB ,MI1,MJ1,MK1) 

TMAT( 1,1)  =  Mil 

TMAT (1,2)  =  MJ1 

TMAT( 1,3)  =  MK1 

TMATTR( 1,1)  =  TMAT(1,1) 

TMATTR( 1,2)  =  TMAT( 2,1) 

TMATTR( 1,3)  =  TMAT(3,1) 

TMATTR( 2,1)  =  TMAT(1,2) 

TMATTR {2,2)  =  TNAT(2,2) 

TMATTR(2,3)  =  TMAT(3,2) 

TMATTR ( 3,1)  =  TNAT(1,3) 

TMATTR( 3,2)  =  TMAT(2,3) 

TMATTR( 3,3)  =  TMAT(3,3) 

DO  375  II  =  1,3 
DO  375  J  =  1,3 
TEMP  =  0. ODO 
DO  370  K  =  1,3 
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370 

375 


378 

380 


390 

■sWfV :?Wc 
V?  <V 

395 


* 

* 


TEMP  =  TMATTR(I1,K)  *  LIMAT(I,K,J)  +  TEMP 
CONTINUE 

MATTMP(Il.J)  =  TEMP 
CONTINUE 


DO  380  II  =  1,3 
DO  380  J  =1,3 
TEMP  =  0. 0D0 
DO  378  K  =  1,3 

TEMP  =  MATTMP(Il.K)  *  TMAT(K, J)  +  TEMP 
CONTINUE 

NIMATC I 1 , J)  =  TEMP 
CONTINUE 


IXXT(I)  =  NIMATC 1,1) 
IXYT(I)  =  NIMATC 1,2) 
IXZT(I)  =  NIMATC 1,3) 
IYYT(I)  =  NIMATC 2, 2) 
IYZT(I)  =  NIMATC 2,3) 
IZZT(I)  =  NIMATC 3, 3) 

CONTINUE 


ENTER  CONSTANTS  INTO  MATRIX  A  AND  B  ***** 
LINK  ONE 


SUM  OF  FORCES 

X  DIRECTION 

MATAC 1,1)  = 

1.  ODO 

MATA (1,4)  = 

Ml 

MATAC 1,10)  = 

-1.  ODO 

MATBCl) 

0.  ODO 

Y  DIRECTION 

MATAC 2, 2)  = 

1.  ODO 

MATAC 2, 5)  = 

Ml 

MATAC 2, 11)  = 

-1.  ODO 

MATB(2) 

0.  ODO 

Z  DIRECTION 

MATA( 3, 3)  = 

1.  ODO 

MATAC 3, 61  = 

Ml 

MATAC 3, 12)  = 

-1.  ODO 

MATBC3) 

-WG1 

EQUATIONS  AT  JOINT  ZERO 

X  DIRECTION 

MATA( 4, 4)  = 

1.  ODO 

MATA( 4, 8)  = 

RBG1( 3) 

MATAC  4,9)  =  • 

-RBG1(2) 

MATB(4) 

AXO  -  MICO 

Y  DIRECTION 

MATAC 5, 5)  = 

1.  ODO 

MATAC 5, 7)  =  • 

-RBG1C3) 

MATAC 5, 9)  = 

RBG1( 1) 

MATBC5) 

AYO  -  MJCO 

Z  DIRECTION 

MATA( 6, 6)  = 

1.  ODO 

MATA( 6, 7)  = 

RBG1( 2) 

MATAC 6, 8)  =  ■ 

-RBG1C 1) 
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MATB(6)  =  AZO  -  MKCO 

*  SUM  OF  MOMENTS  EQUATIONS 

*  X  DIRECTION 


MATA( 7,2) 

= 

RBG1( 3) 

MATA( 7,3) 

= 

-RBG1(2) 

MATA( 7,7) 

= 

-IXXT(l) 

MATA( 7,8) 

= 

IXYT(l) 

MATA( 7,9) 

= 

IXZT(l) 

MATA( 7 , 11) 

= 

-RAG1(3) 

MATA( 7,12) 

= 

RAG1( 2) 

MATB( 7) 

= 

T1X  -  TOX 

Y  DIRECTION 

MATA( 8,1) 

= 

-RBG1( 3) 

MATA( 8,3) 

zr 

RBG1(1) 

MATA( 8,7) 

= 

IXYT( 1) 

MATA( 8,8) 

= 

-IYYT(l) 

MATA( 8,9) 

= 

IYZT(l) 

MATA(8, 10) 

= 

RAG1( 3) 

MATA( 8,12) 

— 

-RAGl(l) 

MATBf  8) 

= 

T1Y  -  TOY 

Z  DIRECTION 

MATA( 9,1) 

= 

RBG1( 2) 

MATA( 9,2) 

— 

-RBG1( 1) 

MATA( 9,7) 

= 

IXZT( 1) 

MATA( 9,8) 

= 

IYZT(l) 

MATA( 9,9) 

= 

-IZZT(l) 

MATA( 9,10) 

= 

-RAG1( 2) 

MATA( 9,11) 

= 

RAG1(1) 

MATB( 9 ) 

= 

T1Z  -  TOZ 

LINK  TWO 


*  SUM  OF  FORCES 


X  DIRECTION 


460 

MATA( 10,10) 

= 

1.  ODO 

MATA( 10,13) 

= 

M2 

MATA( 10,19) 

= 

-1.  ODO 

MATB(IO) 

= 

0.  ODO 

Vc 

Y  DIRECTION 

MATA( 11,11) 

= 

1.  ODO 

MATA( 11, 14) 

= 

M2 

MATA( 11,20) 

-1. ODO 

MATB( 11) 

= 

0.  ODO 

* 

Z  DIRECTION 

MATA( 12,12) 

1.  ODO 

MATA( 12,15) 

= 

M2 

MATA( 12,21) 

= 

-1.  ODO 

MATB( 12) 

= 

-WG2 

EQUATIONS  AT  JOINT  ONE 
X  DIRECTION 


MATA( 13,4)  =  -1. ODC 

MATA( 13,8)  =  -RAG  1(3) 

MATA( 13,9)  =  RAG1( 2) 

MATA( 13,13)  =  1.  ODO 

MATA( 13,17)  =  RBG2( 3) 
MATA( 13,18)  =  -RBG2( 2) 
MATB( 13)  =  MIC1  -  MIC2 

Y  DIRECTION 
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MATAC 14,5) 

= 

-1.  ODO 

MATA( 14,7) 

= 

RAG1(3) 

MATA( 14,9) 

= 

-RAGl(l) 

MATA( 14 , 14) 

= 

1.  ODO 

MATA( 14,16) 

= 

-RBG2(3) 

MATA( 14, 18) 

= 

RBG2C1) 

MATBC 14) 

= 

MJC1  -  MJC2 

?v 

Z  DIRECTION 

MATA( 15,6) 

= 

-1. ODO 

MATA( 15,7) 

= 

-RAG1C2) 

MATA( 15,8) 

= 

RAG1C 1) 

MATA( 15,15) 

= 

1.  ODO 

MATA( 15,16) 

- 

RBG2C2) 

MATAC 15,17) 

S 

-RBG2C1) 

MATBC 15) 

s 

MKC1  -  MKC2 

* 

SUM  OF  MOMENTS  EQUATIONS 

X  DIRECTION 

MATA( 16, 11) 

s 

RBG2C3) 

MATAC lo, 12) 

ss 

-RBG2(2) 

MATAC 16,16) 

= 

-IXXTC2) 

MATAC 16,17) 

= 

I XYT ( 2 ) 

MATAC 16, 18) 

— 

IXZT( 2) 

MATAC 16,20) 

= 

-RAG2C3) 

MATAC 16,21) 

RAG2( 2) 

MATBC 16) 

= 

T2X  -  T1X 

* 

Y  DIRECTION 

MATAC 17, 10) 

= 

-RBG2C  3) 

MATAC 17, 12) 

= 

RBG2C1) 

MATAC 17,16) 

= 

IXYT(2) 

MATAC 17,17) 

= 

-IYYTC2) 

MATAC 17, 18) 

= 

IYZTC  2) 

MATAC 17, 19) 

= 

RAG2C3) 

MATAC 17,21) 

= 

-RAG2C1) 

MATB( 17) 

= 

T2Y  -  T1Y 

V? 

Z  DIRECTION 

MATAC 18, 10) 

= 

RBG2C2) 

MATA( 18,11) 

= 

-RBG2C 1) 

MATA( 18,16) 

= 

IXZTC  2) 

MATAC 18, 17) 

= 

IYZTC 2) 

MATAC 18, 18) 

= 

-IZZTC2) 

MATAC 18, 19) 

-RAG2C2) 

MATAC 18,20) 

= 

RAG2C1) 

MATB( 18) 

= 

T2Z  -  T1Z 

** 

LINK  THREE 

* 

SUM  OF  FORCES 

* 

X  DIRECTION 

530 

MATAC 19, 19) 

= 

1.  ODO 

MATAC 19,22) 

M3 

MATBC 19) 

= 

0.  ODO 

ic 

Y  DIRECTION 

MATAC 20, 20) 

= 

1.  ODO 

MATAC 20, 23) 

= 

M3 

MATBC 20) 

0.  ODO 

Jm 

Z  DIRECTION 

MATAC  21,21) 

=: 

1.  ODO 

MATA( 2 1,24) 

= 

M3 
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MATB (21)  =  -WG3 

*  EQUATIONS  AT  JOINT  TWO 

*  X  DIRECTION 

MATA( 22,13)  =  -l.ODO 
MATA( 22,17)  =  -RAG2C  3) 

MATA( 22,18)  =  RAG2(2) 

MATA(22,22)  =  l.ODO 
MATA( 22,26)  =  RBG3(3) 

MATA( 22,27)  =  -RBG3(2) 

MATB ( 22)  =  MIC3  -  MIC4 

*  Y  DIRECTION 

MATA( 23 , 14)  =  -l.ODO 
MATA( 23,16)  =  RAG2(3) 

MATA( 23,18)  =  -RAG2(1) 

MATA( 23 , 23)  =  l.ODO 
MATA( 23,25)  =  -RBG3(3) 

MATA( 23,27)  =  RBG3(1) 

MATB(23)  =  MJC3  -  MJC4 

*  Z  DIRECTION 

MATA( 24,15)  =  -l.ODO 
MATA(  24,16')  =  -RAG2(2) 

MATA( 24,17  =  RAG2( 1) 

MATA( 24,24)  =  l.ODO 
MATA( 24,25)  =  RBG3(2) 

MATA( 24 , 26)  =  -RBG3(1) 

MATB(24)  =  MKC3  -  MKC4 

*  SUM  OF  MOMENTS  EQUATIONS 

*  X  DIRECTION 

MATA( 25,20)  =  RBG3(3) 

MATA( 25,21)  =  -RBG3(2) 

MATA( 25,25)  =  -IXXT(3) 

MATA( 25,26)  =  IXYT(3) 

MATA( 25,27)  =  IXZT(3) 

MATB(25)  =  -T2X 

*  Y  DIRECTION 

MATA( 26,19)  =  -RBG3(3) 

MATA( 26,21)  =  RBG3(1) 

MATA( 26,25)  =  IXYT(3) 

MATA( 26,26)  =  -IYYT(3) 

MATA( 26,27)  =  IYZT(3) 

MATB( 26)  =  -T2Y 

*  Z  DIRECTION 

MATA( 27 , 19 )  =  RBG3(2) 

MATA( 27 , 20)  =  -RBG3(1) 

MATA( 27 , 25 )  =  IXZT(3) 

MATA( 27,26)  =  IYZT(3) 

MATA( 27,27)  =  -IZZT(3) 

MATBC27)  =  -T2Z 

*****  FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIRECTION  ***** 
***  MODIFIED  BY  RM  VERBOS 
590  DO  600  I  =  4,8 

DO  595  J  =  1,27 
MATA(I,J)  =  0.  ODO 

*  MATA( J , I )  =  0. ODO 

595  CONTINUE 
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600 

605 

610 


MATB(I)  =  0.0D0 
CONTINUE 

MATA(4 ,4)  =  1. 0D0 
MATA( 5,5)  =  1. ODO 
MATA(6 ,6)  =  1.  ODO 
MATA(7,7)  =  1. ODO 
MATA( 8,8)  =  1. ODO 

DO  610  J  =  1,27 

MATA( 18 ,J)  =  0. ODO 
MATA( 27 ,J)  =  0.  ODO 
CONTINUE 


MATA(9 ,9) 
MATA( 18,91 
MATA( 18,18) 
MATB ( 18) 
MATA( 27,9) 
MATA( 27 , 27 ) 
MATB( 27) 


-( IZZT( 1)+IZZT( 2)+IZZT( 3) ) 
-1.  ODO 
1.  ODO 
0.  ODO 
-1.  ODO 
1.  ODO 
0.  ODO 


*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

620  CALL  LEQT2F(MATA,M,N, 1A,MATB , IDGT.WKAREA, IER) 

IF  (IER.  EQ.  0)  THEN 

GOTO  640 

ELSE 

WRITE  (*,624)  IER 
624  FORMAT  (15) 

DO  635  1=1,  27 

WRITE  (*,627)  I 
627  FORMAT  (17) 

DO  631  J  =  1,  27,  3 

WRITE  (*,630)  J ,MATA( I , J) , J+l ,MATA( I , J+l) , J+2 ,MATA( I ,  J+2) 

630  FORMAT  ( 15 ,F13.  5 , 15 ,F13. 5 , 15 ,F13. 5) 

631  CONTINUE 

WRITE  (*,633)  I.MATB(I) 

633  FORMAT  (I3.F15. 5) 

635  CONTINUE 

CALL  ENDJOB 
END  IF 


***  FIND  LCOGX , LCOGY , LCOGZ .TIIETA  VALUES ,WX,WY,WZ 

*  JOINT  ZERO 

640  FXO  =  MATB(l) 

FYO  =  MATB( 2) 

F^  -  MATB(  3) 

*  LINK  ONE 

*  SINCE  LINK1  IS  CONSTRAIN  TO  ROTATE  AROUND  THE  Z  ONLY 

AX1  =  0. ODO 
AY1  =  0. ODO 
AZ1  =  0.  ODO 

*660  AX1  =  MATB(A) 

*  VELX1  =  INTGRL( 0. ,AX1) 

*  LCOGX 1  =  INTGRL( X 1 , VELX 1 ) 

*  LCOGX(l)  =  LCOGX 1 
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* 

AY1 

=  MATB ( 5 ) 

T? 

VELY1 

=  INTGRL( 0.  , AY1) 

* 

LCOGY1 

=  INTGRL( Y 1 , VELY1 ) 

* 

LCOGY(l) 

=  LC0GY1 

■>v 

AZ1 

=  MATB( 6) 

* 

VELZ1 

=  INTGRL( 0. ,AZ1) 

* 

LC0GZ1 

=  INTGRL( Z 1 , VELZ 1 ) 

* 

LCOGZ( 1) 

=  LC0GZ1 

WD1X 

=  MATB ( 7 ) 

W1X 

=  INTGRL( 0.  ,WD1X) 

WDX(l) 

=  WD1X 

Wl(l) 

=  W1X 

WD1Y 

=  MATB ( 8 ) 

W1Y 

=  INTGRL(0. ,WD1Y) 

WDY(l) 

=  WD1Y 

W 1  ( 2 ) 

=  W1Y 

WD1Z 

=  MATB(9) 

W1Z 

=  INTGRL( 0. , WD 1 Z ) 

WDZ(l) 

=  WD1Z 

Wl(  3) 

=  W1Z 

Vr>Wr 

ADDED  BY 

R.  M.  VERBOS 

685 

THZ 

=  INTGRL( 0. ,W1Z) 

THZANG 

=  THZ  * **  RADEG 

COSTHZ 

=  DCOS(THZ) 

SINTHZ 

=  DSIN(THZ) 

*  IF  THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 

*  THE  DIRECTION  COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 

*  AND  DO  NOT  NEED  TO  BE  CALCULATED 

* 

*  CALC  DIRECTIONAL  COSINES  FOR  LINK  ONE 

* 

*700  DRCSX(l)  =  LC0GX1  /  LNC0G1 

*  DRCSY(l)  =  LC0GY1  /  LNC0G1 

*  DRCSZ(l)  =  LC0GZ1  /  LNC0G1 

*  AMP  =  DSQRT(DRCSX( 1)*DRCSX( 1)+DRCSY( 1)*DRCSY( 1)+.  .  . 

*  DRCSZ( 1)*DRCSZ( 1) ) 

*  DRCSX(l)  =  DRCSX( 1)/AMP 

*  DRCSY(l)  =  DRCSY( 1)/AMP 

*  DRCSZ(l)  =  DRCSZ(1)/AMP 

*720  DRCANX(l)  =  DACOS(DRCSX( I ) )  *  RADEG 

*  DRCANY(l)  =  DACOSfDRCSY(l))  *  RADEG 
DRCANZ(l)  =  DACOS(DRCSZ( 1) )  *  RADEG 

* 

**  JOINT  LOCATION 

*730  JX1  =  LINKL1  *  DRCSX(l) 

*  JY1  =  LINKL1  *  DRCSY(l) 

*  JZ1  =  LINKL1  *  DRCSZ(l) 

*  JOINT  ONE 

735  FX1  =  MATB(IO) 

FY1  =  MATB(ll) 

FZ1  =  MATB(12) 

**  LINK  TWO 

740  AX2  =  MATB( 13) 

VELX2  =  INTGRL( 0.  ,AX2) 
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LC0GX2 

= 

INTGRL( X2 , VELX2 ) 

LCOGX( 2) 

— 

LC0GX2 

AY2 

= 

MATB( 14) 

VELY2 

= 

INTGRL(0.  , AY2) 

LC0GY2 

= 

INTGRLI Y2 , VELY2 ) 

LC0GY(2) 

= 

LC0GY2 

AZ2 

= 

MATB( 15) 

VELZ2 

= 

INTGRL( 0.  ,AZ2) 

LC0GZ2 

= 

INTGRL(Z2,VELZ2) 

LC0GZ(2) 

= 

LC0GZ2 

WD2X 

= 

MATB( 16) 

W2X 

= 

INTGRL(0.  ,WT)2X) 

WDX( 2 ) 

= 

WD2X 

W2(  1 ) 

= 

W2X 

WD2Y 

= 

MATB( 17 ) 

W2Y 

= 

INTGRL(0.  ,WD2Y) 

WDY( 2) 

= 

WD2Y 

W2(  2) 

= 

W2Y 

WD2Z 

= 

MATB( 18) 

W2Z 

= 

INTGRLC 0.  ,WD2Z) 

WDZ( 2) 

= 

WD2Z 

W2(  3) 

= 

W2Z 

*  GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 

DRCSXI2)  =  (LCOGX2  -  JX1)  /  LNCOG2 

DRCSYC2)  =  ( LC0GY2  *  JY1)  /  LNCOG2 

DRCSZ( 2)  =  ( LCOGZ2  -  J21)  /  LNCOG2 

790  AMP  =  DSQRT(DRCSX(2)*DRCSX(2HDRCSY(2)*DRCSY(2)+.  . 

DRCSZ(2)*DRCSZ(2)) 

DRCSX( 2)  =  DRCSX( 2) /AMP 

DRCSY( 2)  =  DRCSY( 2) /AMP 

DRCSZ(  2")  =  DRCSZ(  2)/AMP 

DRCANXC  2)  =  DAC0S(DRCSX(2) )  *  RADEG 

DRCANY ( 2  )  =  DACOS(,  DRCSY(  2 ) )  *  RADEG 

DRCANZ(2)  =  DACOS( DRCSZ( 2) )  *  RADEG 

*  JOINT  LOCATION 

800  JX2  =  JX1  +  LINKL2  *  DRCSX(2) 

JY2  =  JY1  +  LINKL2  *  DRCSY(2) 

JZ2  =  JZI  +  LINKL2  *  DRCSZ(2) 

*  JOINT  TWO 

805  FX2  =  MATBQ9) 

FY2  =  MATB( 20) 

FZ2  =  MATB( 21) 

**  LINK  THREE 


AX  3 

MATB( 22) 

VELX3 

= 

INTGRLC 0. 

,AX3) 

LC0GX3 

= 

INTGRLC X3 

, VELX3) 

LCOGX( 3) 

LC0GX3 

AY  3 

= 

MATBC23) 

VELY3 

= 

INTGRLC 0. 

,  AY3) 

LC0GY3 

= 

INTGRLC  Y  3 , VELY3 ) 

LCOGY ( 3 ) 

r- 

LC0GY3 

AZ3 

= 

MATE C 24) 

VELZ3 

= 

INTGRLC 0. 

,  AZ3) 
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LC0GZ3 

INTGRL( Z3 ,VELZ3) 

LC0GZC3) 

= 

LC0GZ3 

WD3X 

= 

MATB(25) 

W3X 

= 

INTGRLCO.  ,WD3X) 

WDX( 3) 

= 

WD3X 

W3C  1) 

= 

W3X 

WD3Y 

= 

MATBC26) 

W3Y 

= 

INTGRLCO.  ,WD3Y) 

WDYC3) 

= 

WD3Y 

W3(  2) 

= 

W3Y 

WD3Z 

= 

MATB(27) 

W3Z 

= 

INTGRLCO.  ,WD3Z) 

WDZ(3) 

= 

WD3Z 

W3(  3) 

= 

W3Z 

*  CALC  DIRECTIONAL  COSINES  FOR  LINK  THREE 
845  DRCSX( 3)  =  ( LCOGX3  -  JX2)  /  LNCOG3 

DRCSY(3)  =  (LCOGY3  -  JY2)  /  LNCOG3 
DRCS2( 3)  =  (LCOGZ3  -  JZ2)  /  LNCOG3 
865  AMP  =  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+.  .  . 

DRCSZ( 3)*DRCSZ( 3) ) 

DRCSX(3)  =  DRCSX( 3)/AMP 

DRCSY( 3)  =  DRCSY( 3 ) /AMP 

DRCSZ( 3)  =  DRCSZ( 3)/AMP 

DRCANX( 3)  =  DACOS(DRCSX( 3) )  *  RADEG 

DRCANY( 3)  =  DACOS(DRCSY( 3) )  *  RADEG 

DRCANZC  3)  =  DACOS(DRCSZ( 3) )  *  RADEG 


*  TIP  LOCATION 

875  TIPX  =  JX2  +  LINKL3  *  DRCSX(3) 

TIPY  *  JY2  +  LINKL3  *  DRCSY(3) 
TIPZ  =  JZ2  +  LINKL3  *  DRCSZ(3) 


* 
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FIND  THE  ANGLE  BETWEEN  THE  LIMKS 
ANG23X  =  DRCANXC  2 )  -  DRCANX(3) 
ANG23Y  =  DRCANY(2)  -  DRCANY( 3) 
ANG23Z  =  DRCANZ( 2)  -  DRCANZC  3) 


ANG12X  =  DRCANX(l) 
ANG12Y  =  DRCANY(l) 
ANG12Z  =  DRCANZC 1) 


DRCANXC  2) 
DRCANYC  2) 
DRCANZC 2) 


DYNAMIC 

NOSORT 

*****  input  TORQUE  AT  JOINTS 

*  CALCULATE  THE  MAGNITUDE  OF  THE  LENGTH  OF  THE  PROJECTION  OF 

*  LINK  2  AND  3  IN  THE  X  Y  PLANE  WHICH  IS  THE  MOMENT  ARM  FOR 

*  THE  CALCULATION  OF  THE  GRAVITATIONAL  TORQUE 

900  MARM2A  =  DSQRT  (  LC0GX2  *  LC0GX2  +  LC0GY2  *  LC0GY2  ) 

MARM2B  =  DSQRT  C  LC0GX3  *  LC0GX3  +  LC0GY3  *  LC0GY3  ) 
MARM3  =  DSQRT  (  ( LC0GX3  -  JX2)  *  C  LC0GX3  -  JX2)  +.  .  . 

CLC0GY3  -  JY2)  *  CLC0GY3  -  JY2)  ) 

*  CALCULATE  GRAVITATIONAL  TORQUES 
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TGI  =  MARM2A  *  WG2  +  MARM2B  *  WG3 
TG2  =  MARM3  *  WG3 


*  INPUT  TORQUES  OVERTOP  OF  GRAVITY 


*10 

T1FNC  = 

2* A  -  A  *  DCOS  (  W  *  TIME  ) 

* 

T2FNC  = 

-3  *  A  *  DSIN  (W*TIME) 

* 

TOZ 

A  *  DSIN  (  W  *  TIME  ) 

* 

TOZ 

A  -  A  *  DSIN  (  W  *  TIME  ) 

* 

TOZ 

50. 0  -  50.  0  *  TIME 

* 

CALCULATE  TOTAL  TORQUE  ON  JOINT  1  AND  2 

T1  =  TGI  +  T1FNC 
T2  =  TG2  +  T2FNC 


*  CALCULATE  X  AND  Y  COMPONENT  OF  TORQUE  ON  JOINT  1  AND  2 

T1X  =  T1  *  DCOS  (THZ) 

T1Y  =  T1  *  DSIN  (THZ) 

T2X  =  T2  *  COS  (THZ) 

T2Y  =  T2  *  SIN  (THZ) 

*  TORQUE  CHECK 

T1CH  =  DSQRT  (  T1X  *  T1X  +  T1Y  *  T1Y  ) 

T2CH  =  DSQRT  (  T2X  *  T2X  +  T2Y  *  T2Y  ) 

END 

STOP 

FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPROD( VECTA , VECTB , MI , MJ , MK ) 

940  IMPLICIT  REAL*8  (A-Z) 

DIMENSION  V'ECTA(  3") ,  VECTB (  3) 

MI  =  VECTA( 2)  *  VECTB ( 3)  -  VECTA(3)  *  VECTB(2) 
MJ  =  VECTA( 3)  *  VECTB ( 1)  -  VECTA(l)  *  VECTB(3) 
MK  =  VECTA(l)  *  VECTB( 2)  -  VECTA(2)  *  VECTB(l) 
RETURN 

END 
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APPENDIX  B 


DOUBLE  PENDULUM  VALIDATION  PROGRAM 


TITLE  DOUBLE  PENDULUM  VALIDATION  PROGRAM 


*  THIS  IS  A  PROGRAM  THAT  WILL  SOLVE  THE  DIRECT  DYNAMICS  PROBLEM  FOR  A  * 

*  2  LINK  RIGID  BODY  MANIPULATOR  WITHOUT  THE  USE  OF  THE  STANDARD  TRAN-  * 

*  FORMATION  MATRICES  AND  VALIDATE  THE  MOTION  BY  THE  USE  OF  THE  DOUBLE  * 

*  PENDULUM  PROBLEM.  * 


* 


LT  R.  M.  VERBOS  USN 


* 


-• _ ! _ 


TERMINAL 
METHOD  ADAMS 

PRINT  .  10 , ANGTH2 ,TH2ANG , ANGTH3 ,TH3ANG , ANG23 

CONTROL  FINTIM  =4.0,  DELT  =  .0005,  DELMAX  =  .1,  DELPRT  =  .10 

SAVE  .05,  ANGTH2 .TH2ANG , ANGTH3 ,TH3ANG , ANG23 

GRAPH  (DE=TEK618)  TIME , ANGTH2 ,TH2ANG 

GRAPH  ( DE=TEK618)  TIME , ANGTH3 .TH3ANG 

GRAPH  (DE=TEK618)  TIME ,ANG23 

D  DIMENSION  MATA( 27,27) ,MASS( 3,2) ,L(3,2) ,RX(3,2) ,RY(3,2) ,RZ(3,2) 

D  DIMENSION  IXX( 3 ,2) , IXZ( 3 , 2) , IXY( 3 , 2) . IYY( 3 ,2) , IYZ( 3 , 2) ,  IZZ(3 , 2) 

D  DIMENSION  IMAT( 3,3,3) ,NIMAT( 3,3), TMAT( 3,3), TMATTR( 3,3), MATTMP (3,3) 
D  DIMENSION  LI.MAT(  3 , 3 , 3) 

FIXED  IER , I , J ,M ,K , P,N , IA, IDGT,A, II 

ARRAY  MATE (27) , ABMATB( 27) , LC0GX( 3) ,LCOGY(3) ,LCOGZ(3) 

ARRAY  VECTA0( 3) , VECTB0( 3) , VECTA1( 3) ,VECTB1( 3) ,VECTA2( 3) , VECTB2( 3) 

ARRAY  VECTA( 3) ,VECTB( 3) 

ARRAY  WDX( 3) ,WDY( 3) ,WDZ( 3) ,W1(3) ,W2(3) ,W3(3) 

ARRAY  RBG1(3) ,RAG1(3) ,RBG2(3) ,RAG2(3) ,RBG3(3) 

ARRAY  WKAREA( 850) 

ARRAY  IXXT( 3) , IYYT( 3) , IZZT( 3) , IXYT( 3) , IXZT( 3) , IYZT( 3) 

ARRAY  DRCANX( 3) , DRCANY ( 3 ) ,DRCANZ(3) 

ARRAY  DRCSX( 3) ,DRCSY(3) ,DRCSZ(3) 

INITIAL 


*****  INPUT  ***** 

*  INPUT  PARAMETER  CONSTANTS 

55  A  =  15. 0D0 

P  =  0. 0D0 
W  =  2  *  PI 

IDGT  =  6 

***  MODIFIED  BY  RM  VERBOS 
G  =  9.81D0 

*  G  =  0.  0D0 
N  =  27 


63 


M  =  1 
IA  =  27 
IER  =  0 
LEVELQ  =  0 
LEVLDQ  =  0 

INPUT  JOINT  LOCATIONS  IN  METERS 
JXO  =  O.ODO 
JYO  =  O.ODO 
JZO  =  0. ODO 
JX1  =  0. ODO 
JY1  =  O.ODO 
JZ1  =  0. 2D0 
JX2  =  0.  01  0 
JY2  =  0.416D0 
JZ2  =  0. 2D0 

INPUT  TORQUE  CONSTANTS 


TOX 

— 

0.  ODO 

TOY 

= 

0.  ODO 

TOZ 

= 

0.  ODO 

T1X 

= 

0.  ODO 

T1Y 

= 

0.  ODO 

T1Z 

= 

0.  ODO 

T2X 

= 

0.  ODO 

T2Y 

= 

0.  ODO 

T2Z 

0.  ODO 

*  INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS 
FOR  EACH  LINK  ENDS 

LC  1,1)  =  0. 05D0 
L( 1 ,2)  =  0. 15D0 
I.(  2 , 1 )  =  0.  20D0 
L( 2 , 2)  =  0. 216D0 
L( 3, 1)  =  0. 225D0 
L(  3 , 2)  =  0. 226D0 

*  INPUT  THE  LINK  LENGTHS  OF  THE  ROBOT 

LINKL1  =  0. 20D0 
LINKL2  =  0.416DO 
LINKL3  =  0.45  IDO 

*  INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 

110  MASS( 1,1)  =  11. ODO 

MASS( 1,2)  =  33. ODO 
MASS( 2,1)  =  2.2D0 
MASS( 2,2)  =  2.  2D0 
MASS( 3,1)  =  28.6D0 
MASS( 3,2)  =  28.6D0 

*  INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 


120  LCOGX(l)  =  O.ODO 

XI  =  LCOGX(l) 

LCOGY(l)  =  O.ODO 
Y 1  =  LCOGY(l) 

LCOGZ(l)  =  0. 10D0 
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Z1 

= 

LCOGZ(l) 

LCOGX( 2) 

= 

0.  ODO 

X2 

LCOGX( 2) 

LC0GY(2) 

= 

0.  208D0 

Y2 

= 

LCOGY ( 2 ) 

LCOGZ( 2) 

= 

0.  20D0 

Z2 

= 

LCOGZ( 2) 

LCOGX( 3) 

= 

0.  ODO 

X3 

s 

LCOGX( 3) 

LCOGY( 3) 

s 

0.  6415D0 

Y3 

= 

LCOGY’  (  3  ) 

LC0GZ(3) 

= 

0.  2D0 

Z3 

=r 

LCOGZ( 3) 

INPUT  MASS  OF  EACH  LINK  IN  KG 
Ml  =  44. 0D0 
M2  =  4.4D0 
M3  =  57.  2DO 

INPUT  ACCELERATIONS  OF  JOINT  ZERO 
AXO  =  0. ODO 
AYO  =  0. ODO 
AZO  =  0.  ODO 


INPUT  THE  INITIAL  DIRECTION  COSINES 
DRCSX(l)  =  0. ODO 
DRCSY(l)  =  0. ODO 
DRCSZ(l)  =  1. ODO 
DRCSX(2)  =  0. ODO 
DRCSY( 2)  =  1.  ODO 
DRCSZ( 2)  =  0. ODO 
DRCSX(3)  =  0. ODO 
DRCSY( 3)  =  1. ODO 
DRCSZ(3)  =  O.ODO 


INPUT  THE  INITIAL  DIRECTION  COSINE  ANGLES 


DRCANX( 1 ) 
DRCANY(l) 
DRCANZ(l) 
DRCANX( 2) 
DRCANY( 2) 
DRCANZ( 2) 
DRCANX( 3 ) 
DRCANY( 3) 
DRCANZ( 3) 


90. ODO 
90. ODO 
0.  ODO 
90. ODO 
0.  ODO 
90.  ODO 
90. ODO 
0.  ODO 
90. ODO 


*****  INITIALIZE  ***** 
*  OMEGA  AND  OMEGA  DOT 
160  DO  170  I  =  1,3 


W1(I) 

=  0.  ODO 

W2(I) 

=  0.  ODO 

W3(I) 

=  O.ODO 

WDX(I) 

=  O.ODO 

WDY(I) 

=  0.  ODO 

WDZ(I) 

=  0.  ODO 

170  CONTINUE 
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THZ  =  0.  0D0 


*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA(I,J)  =  0.  ODO 


175 

CONTINUE 

MATB(I)  =  0.  ODO 

180 

CONTINUE 

VtVoV-jV 

CALCULATIONS  ***** 

yp 

WEIGHTS  C NEWTONS) 

185 

WG1  =  M1*G 

WG2  =  M2*G 

WG3  =  M3*G 

*  COMPUTE  THE  LENGTH  FROM  THE  INBOARD  JOINT  TO  COG 

LNC0G1  =  DSQRT  (  LCOGX( 1)*LC0GX( 1)  +  LCOGY( 1)*LC0GY( 1)  +.  . . 

LC0GZ(1)*LC0GZ(1)  ) 

LX 2  =  LCOGX( 2)  -  JX1 
LY2  =  LCOGY( 2 )  -  JY1 
LZ2  =  LCOGZ( 2)  -  JZ1 

LNC0G2  =  DSQRT  (  LX2*LX2  +  LY2*LY2  +  LZ2*LZ2  ) 

LX 3  =  LCOGX( 3)  -  JX2 
LY3  =  LCOGY( 3)  -  JY2 
LZ3  =  LCOGZ( 3 )  -  JZ2 

LNC0G3  =  DSQRT  (  LX3*LX3  +  LY3*LY3  +  LZ3*LZ3  ) 

COMPUTE  INITIAL  INERTIAS  BASED  ON  POINT  MASSES 
IN  GLOBAL  COORDINATES 
DO  225  I  =  1,3 

RX( 1,1)  =  -L( 1,1)  *  DRCSX(I) 

RX(I,2)  =  L(I,2)  *  DRCSX(I) 

RY ( I , ■ )  =  -L( 1,1)  *  DRCSY(I) 

RY ( I , L )  =  L( 1 ,2)  *  DRCSY(I) 

RZ( 1,1)  =  -LC 1,1)  *  DRCSZ(I) 

RZ( 1,2)  =  L( I , 2 )  *  DRCSZ(I) 

IXX( 1,1)  =  MASSC 1,1)  *  ( ( RY (1,1)  *  RY (1,1))  +  (RZ(I,1)  *  RZ( I ,1))) 

IXX(  1,2)  =  MASS(  1,2)  *  C  (.  RY  (1,2)  *  RY(I,2))  +  (RZ(I,2)  *  RZ(I,2))) 

IXXT(I)  =  TXX( 1,1)  +  IXX( 1,2) 

IYYC 1,1)  =  MASS( 1,1)  *  ( ( RX( 1,1)  *  RX( 1,1))  +  (RZ( 1,1)  *  RZ( 1,1))) 

IYY( 1,2)  =  MASS( 1,2)  *  ( ( RX( 1,2)  *  RX(I,2))  +  (RZ(I,2)  *  RZ(I,2))) 

IYYT(I)  =  IYYC 1,1)  +  IYY( I ,2) 

IZZ(I.l)  =  MASSC 1,1)  *  ( ( RXC 1,1)  *  RXCI.l))  +  (RY(I,1)  *  RY(I,1))) 

I Z Z( 1 ,2)  =  MASSC 1,2)  *  c (RXC 1,2)  *  RX(I,2))  +  (RY(I,2)  *  RYC 1,2))) 

IZZT(I)  =  IZZC 1,1)  +  IZZ( 1,2) 

IXY(I.l)  =  MASS( 1,1)  *  RX( 1,1)  *  RY( I , 1 ) 

IXY( 1,2)  =  MASSC I, 2)  *  RX( 1,2)  *  RYC 1,2) 

IXYT(I)  =  IXY(I,1)  +  IXY( 1,2) 

IXZ(I,1)  =  MASS( 1,1)  *  RZCI.l)  *  RXC 1,1) 

IXZCI.2)  =  MASSC 1,2)  *  RZCI.2)  *  RX(I,2) 

IXZTC I)  =  IXZCI.l)  +  IXZCI.2) 

I YZ ( 1,1)  =  MASSC 1,1)  *  RY( 1,1)  *  RZ( 1,1) 

IYZCI.2)  =  MASSC 1,2)  *  RY( I, 2)  *  RZCI.2) 

IYZT(I)  =  IYZ(I.l)  +  IYZCI.2) 


* 

■jV 

190 


200 


205 
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IF  (IXXT(I)  .  LE.  .01)  THEN 

IXXT(I)  =  .01 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 

IF  (IYYT(I)  .LE.  .02)  THEN 

IYYT(I)  =  .02 

ELSE 

IYYT(I)  =  IYYT(I) 

END  IF 

IF  (IZZT(I)  .  LE.  .  02)  THEN 

IZZT(I)  =  .02 

ELSE 

IZZT(I)  =  IZZT(I) 

END  IF 

IMAT( 1,1,1)  =  IXXT(I) 
IMAT( 1,1,2)  =  IXYT(I) 
IMAT( 1,1,3)  =  IXZT(I) 
IMAT( 1,2,1)  =  -IXYT(I) 
I MAT (1,2,2)  =  IYYT(I) 
IMAT( 1,2,3)  =  IYZT(I) 
IMAT( 1,3,1)  =  -IXZT(I) 
IMAT( 1,3,2)  =  -IYZT(I) 
IMAT(  1,3,3)  =  IZZT(I) 
225  CONTINUE 


DUE  TO  LINK  1  CONSTRAINTS  LINK  1  INERTIAS  ARE  CONSTANT 
IXXT(l)  =  IMAT(  1,1,D 
IXYT(l)  =  IMAT( 1,1,2) 

IXZT(l)  =  IMAT( 1,1,3) 

IYYT(l)  =  IMATt 1,2,2) 

IYZT(l)  =  IMATf 1,2,3) 

IZZT(l)  =  IMAT( 1,3,3) 

TRANSFORM  THE  INITIAL  INERTIAS  TO  LOCAL  COORDINATED 
DO  9  I  =  2,  3 

TMAT( 2,1)  =  -DCOS  (  THZ  ) 

TMAT( 2,2)  =  -DSIN  (  THZ  ) 

TMAT( 2,3)  =  0. ODO 
TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT( 3,3)  =  DRCSZ(I) 

VECTA(l)  =  TMAT(2,1) 

VECTA( 2)  =  TMAT( 2,2) 

VECTA( 3)  =  TMAT( 2,3) 

VECTB(l)  =  TMAT( 3,1) 

VECTB( 2)  =  THAT (3,2) 

VECTB( 3)  =  TMAT( 3,3) 

CALL  CPROD  (VECTA.VECTB ,MI1,MJ1,MK1) 

TMAT( 1,1)  =  Mil 
TMAT( 1,2)  =  MJ1 
TMAT( 1,3)  =  MK1 
TMATTR(l.l)  =  TMAT (1,1) 

TMATTR( 1,2)  =  TMAT(2,1) 

TMATTR( 1,3)  =  TMAT(3,1) 
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TMATTR (2,1)  =  TMAT(1,2) 

TMATTR( 2,2)  =  TMAT(2,2) 

TMATTR ( 2,3)  =  TMAT(3,2) 

TMATTR (3,1)  =  TMAT (1,2) 

TMATTR ( 3,2)  =  TMAT(2,3) 

TMATTR(3,3)  =  TMAT(3,3) 

DO  5  II  =  1,3 
DO  5  J  =1,3 
TEMP  =  O.ODO 
DO  1  K  =1,3 

TEMP  =  TMAT(Il.K)  *  IMAT(I,K,J)  +  TEMP 
1  CONTINUE 

MATTMP( II , J)  =  TEMP 
5  CONTINUE 


DO  8  II  =  1,3 

DO  8  J  =1,3 

TEMP  =  0. ODO 
DO  7  K  =1,3 

TEMP  =  MATTMP(Il.K)  *  TMATTR(K.J)  +  TEMP 

7  CONTINUE 

LIMAT( I , I 1 , J)  =  TEMP 

8  CONTINUE 

9  CONTINUE 

*  DOUBLE  PENDULUM  INITIALIZATION 
TH2D0  =  0  ODO 
TH3D0  =  0. ODO 
TH30  =  PI/2.0 
TH20  =  PI/2. 0 
L2  =  0.  416D0 

L3  =  0. 45  IDO 

M2P  =  30. 8D0 
M3P  =  28. 6D0 
HF  =  0. 5D0 

DERIVATIVE 

NOSORT 

230  CALL  ERRSET  (208,256,-1,1,1) 

CALL  UERSET( LEVELQ , LEVLDQ) 


*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 
DO  240  I  =  1,27 
DO  235  J  =  1,27 

MATA( I , J)  =  0.  ODO 
235  CONTINUE 

MATB(I)  =  O.ODO 
240  CONTINUE 


INPUT  JOINT  EQUATIONS 

JOINT  ZERO  EQUATIONS 
W1  X  (W1  X  RB/G1) 

RBG1( 1)  =  JXO  -  LCOGX( 1) 
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RBG1(2)  =  JYO  -  LCOGY(l) 

RBG] (3)  =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl( 1) 

VECTA( 2)  =  W 1 ( 2 ) 

VECTA( 3)  =  Wl(  3) 

CALL  CPROD( VECTAO ,RBG1 ,MICO ,MJC0 ,MKCO) 
VECTBO(l)  =  MICO 
VECTB0(2)  =  MJCO 
VECTB0(3)  =  MKCO 

CALL  CPROD( VECTAO , VECTBO , MICO , MJCO , MKCO) 
SINCE  LINK  ONE  DOES  NOT  MOVE 

MICO  =  0.  ODO 

MJCO  =  0.  ODO 

MKCO  =  0.  ODO 

INPUT  JOINT  EQUATIONS 
JOINT  ZERO  EQUATIONS 
vn  X  (VI  X  RB/Gl) 

RBG 1(1)  =  JXO  -  LCOGX(l) 

RBG1( 2)  =  JYO  -  LCOGY(l) 

RBG1( 3)  =  JZO  -  LCOGZ(l) 

7ECTA(1 )  =  Wl(l) 

VECTA( 2)  =  W 1 ( 2 ) 

VECTAO)  =  Wl(  3) 

CALL  CPROD( VECTA ,RBG1 , MICO , MJCO , MKCO) 
VECTBO)  =  MICO 
VECTBO)  =  MJCO 
VECTBO)  =  MKCO 

CALL  CPROD( VECTA , VECTB .MICO , MJCO , MKCO ) 

VI  X  (VI  X  RA/G1) 

RAGl(l)  =  JX1  -  LCOGX(l) 

RAG1(2)  =  JY1  -  LCOGY(l) 

RAG1( 3)  =  JZ1  -  LCOGZ(l) 

VECTAO)  =  Vl(l) 

VECTAO)  =  V 1  ( 2 ) 

VECTAO)  =  V 1  ( 3 ) 

CALL  CPROD  (VECTA, RAG1.MIC1.MJC1.MKC1) 
VECTBO)  =  MIC1 
VECTB(2)  =  MJC1 
VECTBO)  =  MKC1 

CALL  CPROD  (VECTA .VECTB, MIC 1.MJC1.MKC1) 

V 2  X  (V2  X  RB/G2) 

RBG2(1)  =  JX1  -  LC0GX(2) 

RBG2( 2)  =  JY1  -  LCOGY(2) 

RBG2( 3)  =  JZ1  -  LCOGZ(2) 

VECTAO  )  =  V2(  1 ) 

VECTA( 2 )  =  V2( 2) 

VECTAO)  =  V2(  3) 

CALL  CPROD  ( VECTA ,RBG2 ,MIC2 ,MJC2 ,MKC2) 
VECTB(l)  =  MIC2 
VECTBO)  =  MJC2 
VECTBO)  =  MKC2 

CALL  CPROD  (VECTA .VECTB ,MIC2 ,MJC2 ,MKC2) 
V2  X  (V2  X  RA/G2) 

RAG2(1)  =  JX2  -  LC0GX(2) 

RAG2( 2 )  =  JY2  -  LC0GY(2) 


RAG2( 3)  =  JZ2  -  LCOGZ( 2) 

VECTA(l)  =  W2( 1) 

VECTA( 2)  =  W2( 2) 

VECTA( 3)  =  W2( 3) 

CALL  CPROD  ( VECTA,RAG2 ,MIC3 ,MJC3 ,MKC3) 
VECTB(l)  =  MIC3 
VECTB( 2)  =  MJC3 
VECTB( 3)  =  MKC3 

CALL  CPROD( VECTA , VECTB , MIC3 , MJC3 , MKC3 ) 
W3  X  (W3  X  RB/G3) 

RBG3C1)  =  JX2  -  LCOGX(3) 

RBG3(2)  =  JY2  -  LCOGY(3) 

RBG3( 3 )  =  JZ2  -  LCOGZ(3) 

VECTA( 1)  =  W3( 1) 

VECTA( 2)  =  W3( 2) 

VECTAC3)  =  W3( 3) 

CALL  CPROD  ( VECTA, RBG3,MIC4,MJC4,MKC4) 
VECTB ( 1 )  =  MIC4 
VECTB( 2)  =  MJC4 
VECTB( 3)  =  MKC4 

CALL  CPROD  (VECTA, VECTB, MIC4,MJC4,MKC4) 

INERTIA  TRANSFORMATION 
DO  390  I  =  2,  3 

TMAT( 2,1)  =  -DCOS  (  THZ  ) 

TMAT( 2,2)  =  -DS1N  (  THZ  ) 

TMAT( 2,3)  =  O.ODO 
TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT( 3,3)  =  DRCSZ(I) 

VECTA(l)  =  TMAT( 2,1) 

VECTAC2)  =  TMAT( 2,2) 

VECTAC  3)  =  TMAT( 2 ,3) 

VECTB ( 1 )  =  THAT( 3,1) 

VECTB(2)  =  TMAT( 3,2) 

VECTB(3)  =  TMAT( 3,3) 

CALL  CPROD  ( VECTA, VECTB , MI  1 ,MJ1 ,MK1) 

TMAT( 1,1)  =  Mil 

TMAT( 1,2)  =  MJ1 

TMAT( 1,3)  =  MK1 

TMATTRC 1,1)  =  TMAT( 1,1) 

TMATTRC 1,2)  =  TMAT(2,1) 

TMATTRC 1,3)  =  TMAT(3,1) 

TMATTRC 2,1)  =  TMAT(1,2) 

TMATTRC 2, 2)  =  TMAT(2,2) 

TMATTRC 2,3)  =  TMAT(3,2) 

TMATTRC 3,1)  =  TMATC 1,3) 

TMATTRC 3, 2)  =  TMAT(2,3) 

■TMATTRC 3, 3)  =  TMATC 3,3) 

DO  375  11  =  1,3 
DO  375  J  =  1,3 
TEMP  =  0. ODO 
DO  370  K  =  1,3 
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370 

375 


378 

380 


390 

■frVnWcVc 

VnV 

* 

Vf 

395 

* 

* 

* 

* 

* 

* 


TEMP  =  TMATTR( I1,K)  *  LIMAT(I,K,J)  +  TEMP 
CONTINUE 

MATTMP(Il.J)  =  TEMP 
CONTINUE 

DO  380  i:  =  1,3 
DO  380  J  =1,3 
TEMP  =  0.  ODO 
DO  378  K  =  1,3 

TEMP  =  MATTMP(Il.K)  *  TMAT(K, J)  +  TEMP 
CONTINUE 

NIMAT(Il.J)  =  TEMP 
CONTINUE 

IXXT(I)  =  NIMAT( 1,1) 

IXYT(I)  =  NIMAT( 1,2) 

IXZT(I)  =  NIMAT( 1,3) 

IYYT(I)  =  NIMAT( 2,2) 

IYZT(I)  =  NIMAT( 2,3) 

IZZT(I)  =  NIMAT( 3,3) 

CONTINUE 

ENTER  CONSTANTS  INTO  MATRIX  A  ***** 

LINK  ONE 


SUM  OF  FORCES 

X  DIRECTION 

MATA(1,1)  = 

1.  ODO 

MATA( 1,4)  = 

Ml 

MATA( 1,10)  = 

-1.  ODO 

MATB(l) 

0.  ODO 

Y  DIRECTION 

MATA(2,2)  = 

1.  ODO 

MATA(2,5)  = 

Ml 

MATA( 2,11)  = 

-1.  ODO 

MATB( 2) 

0.  ODO 

Z  DIRECTION 

MATA( 3,3)  = 

1.  ODO 

MATA(3,6)  = 

Ml 

MATA( 3,12)  = 

-1.  ODO 

MATB ( 3 ) 

-WG1 

EQUATIONS  AT  JOINT  ZERO 

X  DIRECTION 

MATA(4,4)  = 

1.  ODO 

MATA(4,8)  = 

RBG1( 3) 

MATA( 4,9)  =  ■ 

■RBG1(2) 

MATB(4) 

AXO  -  MICO 

Y  DIRECTION 

MATA(5 ,5)  = 

1.  ODO 

MATA( 5,7)  =  ■ 

-RBG1( 3) 

MATA( 5,9)  = 

RBGl(l) 

MATB( 5 ) 

AYO  -  MJCO 

Z  DIRECTION 

MATA(6 ,6)  = 

1.  ODO 

MATA( 6,7;  = 

RBG1(2) 
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MATA( 6 ,8)  =  -RBGl(l) 
MATB ( 6 )  =  AZO  -  MKCO 

*  SUM  OF  MOMENTS  EQUATIONS 


X  DIRECTION 

MATA( 7 ,2) 

= 

RBG1C3) 

MATA (7, 3) 

s 

-RBG1C2) 

MATA( 7,7) 

= 

-IXXT(l) 

MATA( 7,8) 

= 

IXYT(l) 

MATA( 7,9) 

= 

IXZTC1) 

MATA( 7,11) 

= 

-RAG1C3) 

MATA( 7,12) 

= 

RAG1(2) 

MATB( 7) 

= 

T1X  -  TOX 

Y  DIRECTION 

MATA( 8,1) 

= 

-RBG1C3) 

MATA(8 ,3) 

= 

RBGl(l) 

MATA( 8,7) 

= 

IXYTC 1) 

MATA( 8,8) 

'  = 

-IYYTC1) 

MATA( 8,9) 

= 

IYZT(l) 

MATA(8, 10) 

= 

RAG1( 3) 

MATA(8, 12) 

- 

-RAGl(l) 

MATB ( 8 ) 

= 

T1Y  -  TOY 

Z  DIRECTION 

MATA (9,1) 

= 

RBG1(2) 

MATA (9, 2) 

= 

-RBG1C1) 

MATAC 9,7) 

= 

IXZT(l) 

MATA(9 ,8) 

— 

IYZTC1) 

MATA( 9,9) 

= 

-IZZT(l) 

MATA( 9,10) 

= 

-RAG1(2) 

MATA( 9, 11) 

RAGl(l) 

MATB(9) 

= 

T1Z  -  TOZ 

#* 

* 

* 

460 

LINK  TWO 

SUM  OF  FORCES 
X  DIRECTION 
MATA( 10,10) 

1.  ODO 

MATAC 10, 13) 

= 

M2 

MATA( 10,19) 

= 

-1.  ODO 

MATB( 10) 

= 

0.  ODO 

* 

Y  DIRECTION 
MATA( 11, 11) 

1.  ODO 

MATAC 11 , 14) 

= 

M2 

MATAC 11,20) 

= 

-1.  ODO 

MATB ( 11) 

= 

0.  ODO 

* 

Z  DIRECTION 
MATAC 12, 12) 

= 

1.  ODO 

MATAC 12, 15) 

= 

M2 

MATAC 12,21) 

= 

-1. ODO 

MATB(12) 

= 

-WG2 

* 

EQUATIONS  AT  . 

JOINT  ONE 

* 

X  DIRECTION 
MATAC 13, 4) 

= 

-1.  ODO 

MATAC 13,8) 

= 

-RAG1(3) 

MATAC 13,9) 

S5 

RAG1(2) 

MATAC 13, 13) 

= 

1.  ODO 

MATAC 13, 17) 

= 

RBG2C3) 

MATAC 13, 18) 

= 

-RBG2C2) 

MIC1  -  MIC2 


MATBC 13) 

*  Y  DIRECTION 

MATA( 14,5)  =  -1.0D0 

MATAC 14,7)  =  RAG1( 3) 

MATA( 14,9)  =  -RAGl(l) 

MATAC 14,14)  =  l.ODO 
MATAC 14, 16)  =  -RBG2( 3) 
MATAC 14, 18)  =  RBG2C1) 
MATBC 14)  =  MJC1  -  MJC2 

*  Z  DIRECTION 

MATAC 15,6)  =  -l.ODO 

MATAC 15,7)  =  -RAG1C2) 

MATAC 15,8)  =  RAGl(l) 

MATAC 15, 15)  =  1. ODO 

MATAC 15, 16)  =  RBG2(2) 

MATAC 15, 17)  =  -RBG2C1) 
MATBC 15)  =  MKC1  -  MKC2 

*  SUM  OF  MOMENTS  EQUATIONS 

*  X  DIRECTION 

MATAC16.il)  =  RBG2C  3) 
MATAC 16, 12)  =  -RBG2C2) 
MATAC 16, 16)  =  -IXXTC2) 
MATAC 16, 17)  =  IXYT(2) 
MATAC 16, 18)  =  IXZT(2) 
MATAC 16,20)  =  -RAG2C3) 
MATAC 16,21)  =  RAG2C2) 
MATBC 16)  =  T2X  -  T1X 

*  Y  DIRECTION 

MATAC 17, 10)  =  -RBG2( 3) 
MATAC 17,12)  =  RBG2C 1) 
MATAC 17, 16)  =  IXYTC2) 
MATAC 17, 17)  =  -IYYT(2) 
MATAC 17, 18)  =  IYZTC2) 
MATAC 17, 19)  =  RAG2(3) 

MATAC 17, 21)  =  -RAG2(1) 
MATBC 17)  =  T2Y  -  T1Y 

*  Z  DIRECTION 

MATAC 18, 10)  =  RBG2(2) 

MATAC18.il)  =  -RBG2C1) 
MATAC 18, 16)  =  IXZTC2) 
MATAC 18, 17)  =  IYZTC2) 
MATAC 18, 18)  =  -IZZTC2) 
MATAC 18, 19)  =  -RAG2( 2) 
MATAC 18,20)  =  RAG2C1) 
MATBC 18)  =  T2Z  -  T1Z 


LINK  THREE 

* 

SUM  OF  FORCES 

j. 

X  DIRECTION 

530 

MATAC 19,19)  = 

1.  ODO 

MATAC 19,22)  = 

M3 

MATBC 19) 

0.  ODO 

Vf 

Y  DIRECTION 

MATAC 20,20)  = 

1.  ODO 

MATAC 20,23)  = 

M3 
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MATB(20)  =  O.ODO 

*  Z  DIRECTION 

MATA( 21,21)  =  l.ODO 
MATA( 21 , 24)  =  M3 
MATE (21)  =  -WG3 

*  EQUATIONS  AT  JOINT  TWO 

*  X  DIRECTION 

MATA( 22,13)  =  -l.ODO 
MATA( 22,17)  =  -RAG2(3) 

MATA( 22,18)  =  RAG2(2) 

MATA( 22,22)  =  l.ODO 
MATA( 22,26)  =  RBG3(3) 

MATA( 22,27)  =  -RBG3(2) 

MATB( 22)  =  MIC3  -  MIC4 

*  Y  DIRECTION 

MATA( 23 , 14)  =  -l.ODO 
MATA( 23,16)  =  RAG2(3) 

MATA( 23,18)  =  -RAG2(1) 

MATA( 23,23)  =  l.ODO 
MATA( 23,25)  =  -RBG3(3) 

MATA( 23,27)  =  RBG3(1) 

MATB( 23)  =  MJC3  -  MJC4 

*  Z  DIRECTION 

MATA( 24,15)  =  -l.ODO 
MATA( 24,16)  =  -RAG2(2) 

MATA( 24,17)  =  RAG2(1) 

MATA( 24,24)  =  l.ODO 
MATA( 24,25)  =  RBG3(2) 

MATA( 24,26)  =  -RBG3(1) 

MATB( 24)  =  MKC3  -  MKC4 

*  SUM  OF  MOMENTS  EQUATIONS 

*  X  DIRECTION 

MATA( 25,20)  =  RBG3(3) 

MATA( 25,21)  =  -RBG3(2) 

MATA( 25,25)  =  -IXXT(3) 

MATA( 25,26)  =  IXYT(3) 

MATA( 25,27)  =  IXZT(3) 

MATB( 25 )  =  -T2X 

*  Y  DIRECTION 

MATA( 26,19)  =  -RBG3(3) 

MATA( 26,21)  =  RBG3(1) 

MATA( 26,25)  =  IXYT(3) 

MATA( 26,26)  =  -IYYT(3) 

MATA(26,27)  =  IYZT(3) 

MATB(26)  =  -T2Y 

*  Z  DIRECTION 

MATA(27 , 19)  =  RBG3(2) 

MATA( 27 , 20)  =  -RBG3(1) 

MATA( 27,25)  =  IXZT(3) 

MATA( 27 , 26)  =  IYZT(3) 

MATA( 27,27)  =  -IZZT(3) 

MATB( 27)  =  -T2Z 

*****  FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIRECTION  ***** 
590  DO  600  I  =  4,8 

DO  595  J  =  1,27 
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MATA(I,J)  =  O.ODO 
*  MATA( J , I )  =  O.ODO 

595  CONTINUE 

MATB(I)  =  O.ODO 
600  CONTINUE 

MATA(4 ,4)  =  l.ODO 
MATA(  5,5)  =  l.ODO 
MATA(6,6)  =  l.ODO 
MATA( 7,7)  =  1.  ODO 
MATA(  8,8)  =  l.ODO 

605  DO  610  J  =  1,27 

MATA( 18 ,J)  =  O.ODO 
MATA( 27 , J)  =  0. ODO 
610  CONTINUE 

MATA(9 ,9)  =  -( IZZT( 1)+IZZT( 2)+IZZT( 3) ) 

MATA(  18,9)  =  -l.ODO 

MATA( 18,18)  =  1.  ODO 

MATB( 18)  =  O.ODO 

MATA( 27 ,9)  =  -1. ODO 

MATA( 27,27)  =  l.ODO 

MATB(27)  =  O.ODO 


CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

CALL  LEQT2F( MATA , M , N , I A , MATB , IDGT , WKAREA , IER) 

IF  (IER. EQ. 0)  THEN 

GOTO  640 

ELSE 

WRITE  (*,624)  IER 
FORMAT  (15) 

DO  635  1=1,  27 

WRITE  (*,627)  I 
FORMAT  (17) 

DO  631  J  =  1,  27,  3 

WRITE  (*,630)  J ,MATA( I , J) , J+l ,MATA( I , J+l) ,  J+2 ,MATA( I , J+2) 
FORMAT  ( 15, F13.  5,15, F13. 5,15, F13. 5) 

CONTINUE 

WRITE  (*,633)  I ,MATB( I ) 

FORMAT  (I3.F15.5) 

CONTINUE 
CALL  ENDJOB 
END  IF 


FIND  LCOGX , LCOGY , LCOGZ , THETA  VALUES ,WX,WY,WZ 
MODIFIED  BY  R. M. VERBOS 


*  JOINT  ZERO 

640  FXO  =  MATB(l) 

FYO  =  MATB( 2) 

FZO  =  MATB ( 3 ) 

*  LINK  ONE 

*  SINCE  LI NK1  IS  CONSTRAIN  NOT  TO  MOVE  ALL  ACC  AND  VEL  ARE  ZERO 

AX1  =  0. ODO 


*660 

AY1  =  0. 
AZ1  =  0. 
AX1 

ODO 

0D0 

MATBC4) 

* 

VELX1 

= 

INTGRLC  0.  ,AX1) 

* 

LCOGX1 

= 

INTGRLC X1,VELX1) 

* 

LCOGXCD 

= 

LC0GX1 

* 

AY1 

= 

MATBC5) 

* 

VELY1 

= 

INTGRLC 0.  , AY1) 

j. 

LCOGY1 

= 

INTGRL(Y1,VELY1) 

* 

LCOGY(l) 

= 

LC0GY1 

j. 

AZ1 

— 

MATB ( 6 ) 

Vc 

VELZ1 

= 

INTGRLC  0.  , AZI ) 

* 

LCOGZ1 

= 

INTGRLC Zl.VELZl) 

* 

LCOGZC1) 

= 

LC0GZ1 

WD1X 

=  ] 

MATBC7) 

* 

V1X 

— 

INTGRLC 0. ,WD1X) 

WDX(l)  =  WD1X 

Wl(l)  =  W1X 

WD1Y  =  MATB( 8) 

W1Y  =  INTGRLC 0.  ,WD1Y) 

WDY(l)  =  WD1Y 

Vl( 2)  =  W1Y 


* 


* 
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WD1Z  =  MATB(9) 

VIZ  =  INTGRLC 0.  , VD1Z) 

VDZ(l)  =  VD1Z 

V 1 ( 3 )  =  VIZ 

ADDED  BY  R. M. VERBOS 

THZ  =  INTGRLC 0. ,V1Z) 

THZANG  =  THZ  *  RADEG 

COSTHZ  =  DCOS(THZ) 

SIN'THZ  =  DS  INC  THZ) 


*  IF  THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 

*  THE  DIRECTION  COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 

*  AND  DO  NOT  NEED  TO  BE  CALCULATED 

*  CALC  DIRECTIONAL  COSINES  FOR  LINK  ONE 

* 

*700  DRCSX(l)  =  LCOGX1  /  LNCOG1 

*  DRCSY(l)  =  LCOGY1  /  LNCOG1 

*  DRCSZ(l)  =  LCOGZ1  /  LNCOG1 

*  AMP  =  DSQRTCDRCSXC 1)*DRCSX( 1)+DRCSY( 1)*DRCSY( 1)+. . . 

*  DRCSZ( 1 )*DRCSZ( 1 ) ) 

*  DRCSX(l)  =  DRCSX( 1)/AMP 

*  DRCSY(l)  =  DRCSYC  D/AMP 

*  DRCSZ(l)  =  DRCSZC 1)/AMP 

*720  DRCANX(l)  =  DACOSC DRCSX( 1 ) )  *  RADEG 

*  DRCANY(l)  =  DACOS ( DRCSY ( 1 ) )  *  RADEG 

*  DRCANZ(l)  =  DACOSC DRCSZC 1))  *  RADEG 


** 

JOINT 

LOCATION 

*730 

JX1 

=  LINKL1 

* 

DRCSX(l) 

* 

JY1 

=  LINKL1 

* 

DRCSYC 1) 

* 

JZ1 

=  LINKL1 

* 

DRCSZC 1) 
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*  JOINT  ONE 

735  FX1  =  MATB(IO) 

FY1  =  MATB(ll) 
FZ1  =  MATB(12) 
**  LINK  TWO 


AX2 

= 

MATB( 13) 

VELX2 

= 

INTGRL.(0. 

,AX2) 

LCOGX2 

= 

INTGRL(X2 

,VELX2) 

LCOGX( 2) 

= 

LCOGX2 

AY2 

= 

MATB( 14) 

VELY2 

= 

INTGRL( 0. 

>  AY2) 

LCOGY2 

= 

INTGRL(Y2 

, VELY2) 

LCOGY( 2) 

= 

LCOGY2 

AZ2 

= 

MATB( 15 ) 

VELZ2 

= 

INTGRL(0. 

,AZ2) 

LCOGZ2 

= 

INTGRL( Z2 , VELZ2) 

LCOGZ(2) 

= 

LCOGZ2 

WD2X 

= 

MATB( 16) 

W2X 

= 

INTGRL( 0. 

,WD2X) 

WDX( 2) 

= 

WD2X 

W2(  1) 

= 

W2X 

WD2Y 

= 

MATB( 17) 

W2Y 

= 

INTGRL( 0. 

,WD2Y) 

WDY( 2) 

= 

WD2Y 

W  2  ( 2 ) 

= 

W2Y 

WD2Z 

= 

MATB( 18) 

W2Z 

= 

INTGRL( 0. 

,WD2Z) 

WDZ( 2) 

= 

WD2Z 

W2(  3) 

= 

W2Z 

*  GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 

DRCSX( 2)  =  ( LCOGX2  -  JX1 )  /  LNCOG2 

DRCSY( 2)  =  ( LCOGY2  -  JY1)  /  LNCOG2 

DRCSZ(2)  =  ( LCOGZ2  -  JZ1)  /  LNCOG2 

790  AMP  =  DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+.  . 

DRCSZ( 2)*DRCSZ( 2) ) 

DRCSX( 2)  =  DRCSX( 2 ) /AMP 

DRCSY(  2)  =  DRCSY’C  2)/AMP 

DRCSZ( 2)  =  DRCSZ( 2) /AMP 

DRCANXC 2)  =  DACOS(DRCSX( 2) )  *  RADEG 

DRCANYC  2)  =  DACOS( DRCSY( 2 ) )  *  RADEG 

DRCANZ( 2)  =  DACOS(DRCSZ( 2) )  *  RADEG 

*  JOINT  LOCATION 

800  JX2  =  JX1  +  LINKL2  *  DRCSX(2) 

JY2  =  JY1  +  LINKL2  *  DRCSY(2) 

JZ2  =  JZ1  +  LINKL2  *  DRCSZ(2) 

*  JOINT  TWO 

805  FX2  =  MATB(19) 

FY2  =  MATB(20) 

FZ2  =  MATB( 21) 

**  LINK  THREE 

812  AX 3  =  MATB( 22) 

VELX3  =  INTGRL( 0.  ,AX3) 


LCOGX3 

= 

INTGRLC X3.VELX3) 

LC0GXC3) 

LCOGX3 

AY  3 

= 

MATBC23) 

VELY3 

= 

INTGRLC  0.  , AY3) 

LCOGY3 

— 

INTGRLC Y3 , VELY3) 

LCOGY( 3) 

= 

LC0GY3 

AZ3 

= 

MATBC24) 

VELZ3 

= 

INTGRLC 0.  ,AZ3) 

LCOGZ3 

= 

INTGRLC Z3.VELZ3) 

LCOGZC  3) 

= 

LC0GZ3 

WD3X 

— 

MATBC25) 

W3X 

= 

INTGRLC 0.  ,WD3X) 

WDX(3) 

= 

WD3X 

W3C  1) 

=r 

W3X 

WD3Y 

= 

MATBC26) 

W3Y 

= 

INTGRLC 0.  ,WD3Y) 

WDY ( 3 ) 

:= 

WD3Y 

W3(  2) 

= 

W3Y 

WD3Z 

MATB(27) 

W3Z 

= 

INTGRLC 0.  ,WD3Z) 

WDZC  3) 

= 

WD3Z 

W3C  3) 

W3Z 

*  CALC  DIRECTIONAL  COSINES  FOR  LINK  THREE 

845  DRCSX( 3)  =  ( LCOGX3  -  JX2)  /  LNCOG3 

DRCSY( 3)  =  ( LCOGY3  -  JY2 )  /  LNCOG3 
DRCSZ( 3)  =  ( LCOGZ3  -  JZ2)  /  LNCOG3 
865  AMP  =  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. 

DRCSZ( 3)*DRCSZ( 3) ) 

DRCSX( 3)  =  DRCSX( 3) /AMP 

DRCSY(3)  =  DRCSY( 3 ) /AMP 

DRCSZ(3)  =  DRCSZ( 3) /AMP 

DRCANXC  3)  =  DACOS( DRCSX( 3) )  *  RADEG 

DRCANY(3)  =  DACOS( DRCSY( 3 ) )  *  RADEG 

DRCANZ( 3)  =  DACOS(DRCSZ( 3) )  *  RADEG 

*  TIP  LOCATION 

*875  TIPX  =  JX2  +  LINKL3  *  DRCSX(3) 

*  TIPY  =  JY2  +  LINKL3  *  DRCSY(3) 

*  TIPZ  =  JZ2  +  LINKL3  *  DRCSZ(3) 

*  FIND  THE  ANGLE  BETWEEN  THE  LIMKS 

880  ANG23X  =  DRCANX(2)  -  DRCANX(3) 

ANG23Y  =  DRCANY ( 2 )  -  DRCANY( 3) 

ANG23Z  =  DRCANZ( 2)  -  DRCANZ(3) 

ANG23  =  ANG23X  +  ANG23Y  +  ANG23Z 


*  ANG12X  =  DRCANXC 1)  -  DRCANXC 2) 

*  ANG12Y  =  DRCANY(l)  -  DRCAN'YC 2) 

*  ANG12Z  =  DRCANZ(l)  -  DRCANZ(2) 

*  DOUBLE  PENDULUM  CALCULATION 

PA  =  L3*M3P*SIN(TH2-TH3)*TH3DOT**2 

PB  =  SIN(TH2)*G*(M3P+M2P) 

PC  =  L3*M3P*COS  C  TH2 -TH3 ) 

PD  =  L2*(M3P+M2P) 
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PE  =  L2*SIN( TH2-TH3)*TH2D0T**2 
PF  =  SIN(TH3)*G 
PG  =  COS(TH2-TH3)*L2 
PH  =  L3 

TH3DDT  =  (PE-PF+(  (PG*PA+PG,VPB)/PD)  )/(PH-(PG*PC/PD)  ) 
TH2DDT  =  (-PA  -  PB  -  PC*TH3DDT)/PD 
TH3D0T  =  INTGRL(TH3D0,TH3DDT) 

TH2D0T  =  INTGRL(TH2D0 .TH2DDT) 

TH3  =  I’TGRL(TH30 ,TH3D0T) 

TH2  =  INTGRL(TH20,TH2DOT) 

TH2ANG  =  TH2  *  RADEG 
IF(TH2ANG. GT.  0.  0)THEN 
ANGTH2  =  180.0  -  DRCANZ(2) 

ELSE 

ANGTH2  =  DRCANZ( 2)  -  180.0 
END  IF 

TH3ANG  =  TH3  *  RADEG 
IF(TH3ANG.  GT.  0.  0)THEN 
ANGTH3  =  180.0  -  DRCANZ(3) 

ELSE 

ASGTH3  =  DRCANZ( 3)  -  180.0 
END  IF 


END 

STOP 

FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPROD( VECTA , VECTB , MI , M J , MK) 

940  IMPLICIT  REAL*6  (A-Z) 

DIMENSION  VECTAC3) ,VECTB(3) 

MI  =  VECTA(2)  *  VECTB( 3)  -  VECTA(3)  *  VECTB(2) 

MJ  =  VECTA( 3)  *  VECTB ( 1)  -  VECTA(l)  *  VECTB(3) 

MK  =  VECTA(l)  *  VECTB( 2)  -  VECTA(2)  *  VECTB(l) 

RETURN 

END 
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APPENDIX  C 


THREE  DIMENSION  VALIDATION  PROGRAM 


TITLE  NONSINGULAR  3-D  VALIDATION  PROGRAM 


* 


* 


* 

* 


THIS  IS  A  PROGRAM  WHICH  VALIDATES  THE  NON-SINGULARITY  OF  A  THREE  * 
LINK  RIGID  BODY  MANIPULATOR  WITHOUT  THE  USE  OF  THE  STANDARD  ROBOT  * 
TRANSFORMATION  MATRICES.  THE  ORIGINAL  PROGRAM  WAS  WRITTEN  BY  LT  * 
ATINOK  AND  HAS  BEEN  GREATLY  MODIFIED  TO  INCLUDE  THREE  DIMENSIONAL  * 
MOTION  AND  GRAVITY  BY  LT  ROBERT  M.  VERBOS.  SEPTEMBER  1988  * 


Jfy.  y.-jj.-j--'-  -u  j. 


•-I, 


TERMINAL 
METHOD  ADAMS 

PRINT  . 02 ,ERR0R1 ,ERR0R2 .ERROR3 ,  ANG12 ,  ANG23 

CONTROL  FI.NTIM  =1.6,  DELT  =  .0005,  DELMAX  =  .  1,  DELPRT  =  .02 

D  DIMENSION  MATA(27,27) ,MASS(3,2) ,L( 3 ,2) ,RX( 3 , 2) ,RY( 3 , 2) ,RZ( 3 , 2) 

D  DIMENSION  IXX( 3 , 2) , IXZ( 3 , 2) , IXY( 3 , 2 ) , IYY( 3 , 2) , IYZ( 3 , 2) , IZZ(  3 , 2) 

D  DIMENSION  IMAT( 3,3,3), NIMAT( 3,3), TMAT( 3,3), TMATTRC 3,3), MATTMP( 3,3) 

D  DIMENSION  LIMAT(3,3,3) 

FIXED  IER , I , J ,M ,K ,P ,N, IA , IDGT, A , I 1 

ARRAY  MATB(27) ,MATC(27) ,MATD(27) ,LCOGX(3) ,LCOGY(3) ,LCOGZ(3) 

ARRAY  VECTA( 3 ) , VECTB( 3) 

ARRAY  WDX( 3) ,WDY(3) ,WDZ(3) ,W1(3) ,W2(3) ,W3(3) 

ARRAY  RBG1( 3) ,RAG1(3) ,RBG2(3) ,RAG2(3) ,RBG3(3) 

ARRAY  WKAREA( 850) 

ARRAY  IXXT( 3) , IYYT( 31 , IZZTC  3) , IXYT( 3) , IXZT( 3) , IYZT( 3) 

ARRAY  DRCANX( 3 ) , DRCANY( 3) ,DRCANZ(3) 

ARRAY  DRCSX( 3) ,DRCSY(3) ,DRCSZ(3) , 

INITIAL 

j  NPUT 

*  INPUT  PARAMETER  CONSTANTS 

55  A  15. 0D0 

P  =0. 0D0 
W  =  PI/2.0 

IDGT  =  6 

G  =  9.81D0 

N  =  27 

M  =  1 

IA  =  27 

IER  =  0 

LEVELQ  =  0 
LEVLDQ  =  0 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  O.ODO 
JYO  =  0.  ODO 
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■>V 


83 


90 


It 


* 


* 


JZO  =  0. ODO 
JX1  =  0. ODO 
JY1  =  0. ODO 
JZ1  =  0.  2D0 
JX2  =  0. ODO 
JY2  =  0.416D0 
JZ2  =  0.  2D0 

INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS 
L(l,l)  =  0.05D0 
L( 1,2)  =  0. 15D0 
L( 2 , 1 )  =  0. 208D0 
L( 2 , 2)  =  0. 208D0 
L( 3 , 1)  =  0. 2255D0 
L( 3 , 2)  =  0.  2255D0 
INPUT  THE  LINK  LENGTHS 
LINKL1  =  0.  20D0 
LINKL2  =  0.416D0 
LINKL3  ^  0.45 IDO 

INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 
MASS( 1,1)  =  1.  ODO 
MASS( 1,2)  =  3. ODO 
MASS( 2,1)  =  2. 2D0 
MASS( 2,2)  =  2. 2D0 
MASS ( 3 , 1)  =  8. 6D0 
MASS( 3,2)  =  8. 6D0 

INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 


LCOGX(l) 

= 

0.  ODO 

XI 

= 

LCOGX(l) 

LCOGY(l) 

= 

0.  ODO 

Y1 

= 

LCOGYC 1) 

LCOGZ(l) 

= 

0.  10D0 

Z1 

- 

LCOGZ(l) 

LC0GX(2) 

= 

0.  ODO 

X2 

LCOGX’C  2) 

LC0GY( 2) 

= 

0.  208D0 

Y2 

LCOGYC 2) 

LC0GZ( 2) 

= 

0. 20D0 

Z2 

= 

LC0GZC2) 

LCOGX'C  3) 

0.  ODO 

X3 

= 

LCOGXC  3) 

LCOGYC  3) 

= 

0.  6415D0 

Y3 

= 

LCOGYC 3) 

LCOGZ( 3) 

= 

0.  2D0 

Z3 

= 

LC0GZC3) 

INPUT  MASS  OF  EACH  LINK  IN  KG 
Ml  =  4. ODO 
M2  =  4. 4D0 
M3  =  17. 2D0 

INPUT  ACCELERATIONS  OF  JOINT  ZERO 
AXO  =  0. ODO 
AYO  =  0. ODO 
AZO  =  0. ODO 

INPUT  THE  INITIAL  DIRECTION  COSINES 
DRCSX( 1)  =  0. ODO 
DRCSY( 1)  =  0. ODO 
DRCSZ( 1)  =  1. ODO 
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DRCSX( 2)  =  0. 0D0 
DRCSY(2)  =  l.ODO 
DRCS2(2)  =  0. ODO 
DRCSX( 3)  =  0.  ODO 
DRCSY(3)  =  l.ODO 
DRCSZ(3)  =  0. ODO 

*  INPUT  THE  INITIAL  DIRECTION  COSINE  ANGLES 

DRCANX(l)  =  90. ODO 
DRCANY ( 1 )  =  90. ODO 
DRCANZ(l)  =  0.  ODO 

DRCANX( 2 )  =  90. ODO 
DRCANY(2)  =  0.  ODO 

DRCANZ( 2)  *  90. ODO 
DRCANX( 3)  =  90. ODO 
DRCANY( 3)  =  0. ODO 

DRCANZC3)  =  90. ODO 

*****  INITIALIZE  ***** 

*  OMEGA  AND  OMEGA  DOT 

139  DO  146  I  =  1,3 

W1(I)  =  0. ODO 

W2( I )  =  0.  ODO 

W3(I)  =  0. ODO 

VDX( I )  =  0. ODO 
WDY(I)  =  0. ODO 
WDZ(I)  =  0.  ODO 
146  CONTINUE 

Wl( 3)  =  2.  ODO 

W2( 1)  =  2. ODO 

W3( 1)  =  2. ODO 

WDY( 2 )  *  4.  ODO 
WDY ( 3 )  =  4.  ODO 

THZ  =  0.  ODO 

*  INITIALIZE  MATRICES  TO  ZERO 

DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA(I,J)  =  0.  ODO 
175  CONTINUE 

MATB(I)  =  0.  ODO 
MATC(I)  =  0.  ODO 
MATD(I)  =  0.  ODO 
180  CONTINUE 

*****  CALCULATIONS  ***** 

*  WEIGHTS  (NEWTONS) 

185  WG1  =  M1*G 

WG2  =  M2*G 
WG3  =  M3*G 

*  COMPUTE  THE  LENGTH  FROM  THE  INBOARD  JOINT  TO  COG 

LNC0G1  =  DSQRT  (  LCOGX( 1)*LC0GX( 1)  +  LCOGYC 1)*LC0GY( 1)  +. 

LCOGZ( 1)*LC0GZ( 1)  ) 

LX 2  =  LCOGX( 2)  -  JX1 
LY2  =  LCOGYC 2)  -  JY1 
LZ2  =  LC0GZ(2)  -  JZ1 

LNC0G2  =  DSQRT  (  LX2*LX2  +  LY2*LY2  +  LZ2*LZ2  ) 
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LX 3  =  LCOGX( 3)  -  JX2 
LY3  =  LCOGY( 3)  -  JY2 
LZ3  =  LC0GZC3)  -  JZ2 

LNC0G3  =  DSQRT  (  LX3*LX3  +  LY3*LY3  +  LZ3*LZ3  ) 

*  COMPUTE  FBD  INERTIAS  BASED  ON  POINT  MASSES  IN  GLOBAL  COORDINATES 

190  DO  225  I  =  1,3 

RX( 1,1)  =  -L( 1,1)  *  DRCSX(I) 

RX( 1,2)  =  L( I ,2)  *  DRCSX(I) 

RY( I , 1 )  =  -L( 1,1)  *  DRCSY(I) 

RY( I ,2)  =  L( I , 2)  *  DRCSY(I) 

RZ( 1,1)  =  - L( 1,1)  *  DRCSZ(I) 

RZ( 1,2)  =  L( I ,2)  *  DRCSZ(I) 

200  IXX(I,1)  =  MASS( 1,1)  *  ( ( RY( I , 1)  *  RY(I,1))  +  (RZ(I,1)  *  RZ(I,1))) 

IXX( 1,2)  =  MASS( 1,2)  *  ((RY( 1,2)  *  RY(I,2))  +  (RZ(I,2)  *  RZ(I,2))) 

IXXT(I)  =  IXX(I,1)  +  IXX( 1,2) 

IYY( 1,1)  =  MASS(I,1)  *  ((RX(I.l)  *  RX( 1,1))  +  (RZ( 1,1)  *  RZ(I,1))) 

IYY( 1,2)  =  MASS( 1,2)  *  ( (RX( 1,2)  *  RX(I,2))  +  (RZ(I,2)  *  RZ(I,2))) 

IYYT(I)  =  IYY(I.l)  +  IYY( 1,2) 

IZZ(I.l)  =  MASS( 1,1)  *  ((RX(I.l)  *  RX( 1,1))  +  (RY(I,1)  *  RY(I,1))) 

IZZ( 1,2)  =  MASS( 1,2)  *  ( ( RX( 1,2)  *  RX(I,2))  +  (RY(I,2)  *  RY(I,2))) 

IZZT(I)  =  IZZ(I.l)  +  IZZ( 1,2) 

205  IXY( 1,1)  =  MASS( 1,1)  *  RX(I,1)  *  RY(I,1) 

IXY( 1,2)  =  MASS( 1,2)  *  RX( 1,2)  *  RY(I,2) 

IXYT(I)  =  IXY( 1,1)  +  IXY( 1,2) 

IXZ(I,1)  =  MASS( 1,1)  *  RZ( 1,1)  *  RX( 1,1) 

IXZ( 1,2)  =  MASS( 1,2)  *  RZ( 1,2)  *  RX(I,2) 

IXZT(I)  =  IXZ( 1,1)  +  IXZCI.2) 

IYZ( 1,1)  =  MASS( 1,1)  *  RY( 1,1)  *  RZ(I.l) 

IYZ( 1,2)  =  MASS( 1,2)  *  RY( I , 2)  *  RZ(I,2) 

IYZT(I)  =  IYZ(I.l)  +  IYZ( 1,2) 

IF  (IXXT(I)  .LE.  .01)  THEN 

IXXT(I)  =  .01 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 

IF  (IYYT(I)  .LE.  .01)  THEN 

IYYT(I)  =  .01 

ELSE 

IYYT(I)  =  IYYT(I) 

END  IF 

IF  (IZZT(I)  . LE.  . 01)  THEN 

IZZT(I)  =  .01 

ELSE 

IZZT(I)  =  IZZT(I) 

END  IF 

IMAT( 1,1,1)  =  IXXT(I) 

IMAT( 1,1,2)  =  IXYT(I) 

IMAT( 1,1,3)  =  IXZT(I) 

IMAT( 1,2,1)  =  -IXYT(I) 

IMAT( 1,2,2)  =  IYYT(I) 

IMAT( 1,2,3)  =  IYZT(I) 

IMAT( 1,3.1)  =  -IXZT(I) 

IMAT( 1,3,2)  =  -IYZT(I) 

IMAT( 1,3,3)  =  IZZT(I) 

225  CONTINUE 

*  DUE  TO  LINK  1  CONSTRAINTS  AND  SYMMETRY  LINK  1  INERTIAS  ARE  CONST 


83 


IXXT(l)  =  IMAT( 1,1,1) 

IXYT(l)  =  IMAT( 1,1,2) 

IXZT(l)  =  IMAT( 1,1,3) 

IYYT(l)  =  IMAT( 1,2,2) 

IYZT(l)  =  IMAT( 1,2,3) 

IZZT(l)  =  IMAT( 1,3,3) 

*  TRANSFORM  THE  GLOBAL  INERTIAS  TO  LOCAL  WHICH  REMAIN  CONSTANT 
DO  281  I  =  2,  3 

TMAT( 2,1)  =  -DUOS  (  THZ  ) 

TMAT( 2,2)  =  -DSIN  (  THZ  ) 

TMAT(2,3)  =  0.  ODO 
TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT( 3,3)  =  DRCSZ(I) 

VECTA(l)  =  TMAT( 2,1) 

VECTA( 2)  =  TMAT( 2,2) 

VECTA( 3)  =  TMAT( 2,3) 

VECTB(l)  =  TMAT( 3,1) 

VECTB(2)  =  TMAT( 3,2) 

YECTB( 3)  =  TMAT( 3,3) 

CALL  CPROD  ( VECTA , VECTB ,MI1,MJ1,MK1) 

TMAT( 1,1)  =  Mil 
TMAT( 1,2)  =  MJ1 
TMAT( 1 ,3)  =  MK1 
TMATTRC 1,1)  =  TMAT(l.l) 

TMATTRC 1,2)  =  TMAT(2,1) 

TMATTRC 1,3)  =  TMAT(3,1) 

TMATTRC  2,1)  =  TMAT(1,2) 

TMATTR( 2,2)  =  TMAT(2,2) 

TMATTRC 2, 3)  =  TMAT(3,2) 

TMATTRC  3,1)  =  TMATC 1,3) 

TMATTRC 3, 2)  =  TMATC 2, 3) 

TMATTRC 3, 3)  =  TMATC 3, 3) 

DO  272  II  =  1,3 
DO  272  J  =  1,3 
TEMP  =  0. ODO 
DO  270  K  =  1,3 

TEMP  =  TMATC 1 1 ,K)  *  IMATCI,K,J)  +  TEMP 
270  CONTINUE 

MATTMP(Il.J)  =  TEMP 
272  CONTINUE 

DO  280  II  =  1,3 
DO  280  J  =  1,3 
TEMP  =  0.  ODO 
DO  278  K  =  1,3 

TEMP  =  MATTMP(I1,K)  *  TMATTRC K,J)  +  TEMP 
278  CONTINUE 

LIMATC I , II , J)  =  TEMP 

280  CONTINUE 

281  CONTINUE 

DERIVATIVE 

NOSORT 

287  CALL  ERRSET  (208,256,-1,1,1) 

CALL  UERSETC  LEVELQ , LEVLDQ) 

*  INITIALIZE  MATRICES  TO  ZERO 
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DO  293  J  =  1,27 

MATA( I , J)  =  O.ODO 
293  CONTINUE 

MATB(I)  =  0.  ODO 
MATC(I)  =  O.ODO 
MATD(I)  =  O.ODO 
297  CONTINUE 


*  VALIDATION  PORTION  OF  PROGRAM  *** 

*  ENTER  THE  ANGULAR  AND  LINEAR  ACCELERATIONS,  AND  FORCES  INTO  MATRIX  C 

*  FOR  THE  CONSTRAIN  OF  THE  NEPTUNE  II 

TH1L  =  2.0  *  TIME  +  PI/2.0 

W1L  =  2.  ODO 

WD1L  =  O.ODO 

TH1ANG  =  TOIL  *  RADEG 

WDX1G  =  0. ODO 

WDY1G  =  0. ODO 

WDZ1G  =  WD1L 

WX1G  =  0. ODO 

WY1G  =  O.ODO 

WZ1G  =  W1L 

TH2L  =  2. 0  *  TIME 

W2L  =  2.  0 

WD2L  =  0.  ODO 

TH2ANG  =  TH2L  *  RADEG 

WX2G  =  W2L  *  SIN(THIL) 

WY2G  =  W2L  *  (-  COS(THIL)  ) 

WZ2G  =  W1L 

VECTA(l)  =  O.ODO 

VECTA(2)  =  O.ODO 

VECTA( 3)  =  WZ2G 

VECTB(l)  =  VX2G 

VECTB( 2)  =  VY2G 

VECTB( 3)  =  O.ODO 

CALL  CPROD  ( VECTA,VECTB ,MI1 ,MJ1 ,MK1) 

WDX2G  =  Mil 

WDY2G  =  MJ1 

WDZ2G  =  MK1 

TH3L  =  0. ODO 

W3L  =  0.  ODO 

WD3L  =  0.  ODO 

TH3ANG  =  TO3L  *  RADEG 

WX3G  =  WX2G 

WY3G  =  WY2G 

WZ3G  =  WZ2G 

WDX3G  =  WDX2G 

WDY3G  =  WDY2G 

WDZ3G  =  WDZ2G 

R2X  =  LNC0G2  *  C0S(TH2L)  *  COS(THIL) 

R2Y  =  LNC0G2  *  C0S(TO2L)  *  SIN(TOIL) 

R2Z  =  LNC0G2  *  SIN(TH2L) 

RJ2X  =  LINKL2  *  C0S(TO2L)  *  COS(TOIL) 

RJ2Y  =  LINKL2  *  C0S(TH2L)  *  SIN(TOIL) 

RJ2Z  =  LINKL2  *  SIN(TH2L) 

R3X  =  RJ2X  +  LNC0G3  *  C0S(TO2L+TH3L)  *  COS(TOIL) 


R3Y  =  RJ2Y  +  LNC0G3  *  C0S(TH2L+TH3L)  *  SIN(THIL) 

R3Z  =  RJ2Z  +  LNC0G3  *  SIN(TH2L+TH3L) 

LTIPX  =  RJ2X  +  LINKL3  *  C0S(TH2L+TH3L)  *  COS(THIL) 

LTIPY  =  RJ2Y  +  LINKL3  *  COS(TH2L+TH3L)  *  SIN(THIL) 

LTIPZ  =  RJ2Z  +  LINKL3  *  S IN( TH2L+TH3L)  +  JZ1 

CALCULATE  THE  GLOBAL  ACCELERATIONS 
AX1G  =  0.  0D0 

AY1G  =  0.  ODO 

AZ1G  =  0. ODO 

VECTA(l)  =  WDX2G 
VECTA(2)  =  WDY2G 
V£CTA( 3)  =  WDZ2G 
VECTB(l)  =  R2X 
VECTB( 2)  =  R2Y 
VECTB(3)  =  R2Z 

CALL  CPROD  (VECTA, VECTB ,MI1,MJ1,MK1) 

VECTA(l)  =  WX2G 
VECTA(2)  =  WY2G 
VECTA(3)  =  WZ2G 

CALL  CPROD  (VECTA, VECTB , MI 2 ,MJ2 ,MK2) 

VECTB(l)  =  MI2 
VECTB( 2)  =  HJ2 
VECTB( 3)  =  MK2 

CALL  CPROD  (VECTA, VECTB, MI 2, MJ2,MK2) 

AX2G  =  Mil  +  MI 2 

AY2G  =  MJ1  +  MJ2 

AZ2G  =  MK1  +  MK2 

VECTA(l)  =  WDX3G 
VECTA(2)  =  WDY3G 
VECTA(3)  =  WDZ3G 
VECTB(l)  *  R3X 
VECTB( 2)  =  R3Y 
VECTB(3)  =  R3Z 

CALL  CPROD  (VECTA, VECTB, Mil, MJ1.MK1) 

VECTA(l)  =  WX3G 
VECTA( 2)  =  WY3G 
VECTA( 3)  =  WZ3G 

CALL  CPROD  (VECTA, VECTB, MI 2, MJ2.MK2) 

VECTB ( 1)  =  MI2 
VECTB( 2)  =  MJ2 
VECTB( 3)  =  MK2 

CALL  CPROD  (VECTA, VECTB, MI 2, MJ2.MK2) 

AX3G  =  Mil  +  MI2 

AY3G  =  MJ1  +  MJ2 

AZ3G  =  MK1  +  MK2 

SET  MATRIX  D 

MATD( 27)  =  WDZ3G 

MATD( 26)  =  WDY3G 

MATD( 25)  =  WDX3G 

MATD(24)  =  AZ3G 
MATD( 23)  =  AY3G 

MATD( 22)  =  AX3G 

MATD(21)  =  -M3  *  AZ3G  -  WG3 
MATD( 20)  =  -M3  *  AY3G 
MATD( 19)  =  -M3  *  AX3G 
MATD( 18)  =  WDZ2G 
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MATD( 17) 

= 

VDY2G 

MATD( 16) 

= 

WDX2G 

MATD( 15 ) 

= 

AZ2G 

MATD( 14) 

= 

AY2G 

MATD( 13) 

= 

AX2G 

MATD( 12) 

MATD( 21) 

-  M2 

* 

AZ2G 

-  WG2 

MATD( 11) 

= 

MATD(20) 

-  M2 

* 

AY2G 

MATD( 10) 

= 

MATD( 19) 

-  M2 

* 

AX2G 

MATD( 9) 

= 

WDZ1G 

MATD( 8) 

= 

WDY1G 

MATD( 7 ) 

= 

WDX1G 

MATD( 6) 

r~ 

AZ1G 

MATD( 5 ) 

= 

AY1G 

MATD(4) 

= 

AX1G 

MATD( 3) 

MATD( 12) 

-  Ml 

* 

AZ1G 

-  WG1 

MATD(2) 

= 

MATD(ll) 

-  Ml 

* 

AY1G 

MATD(l) 

= 

MATD(IO) 

-  Ml 

* 

AX1G 

***  DIRECT  DYNAMICS  PORTION  OF  PROGRAM  *** 

*  INPUT  JOINT  EQUATIONS 

*  JOINT  ZERO  EQUATIONS 

*  W1  X  (W1  X  RB/G1) 

RBGl(l)  =  JXO  -  LCOGX(l) 

RBG1(2)  =  JYO  -  LCOGY(l) 

RBG1(3)  =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA( 2)  =  W 1 ( 2 ) 

VECTA( 3)  =  Wl( 3) 

CALL  CPROD( VECTA , RBG1 , MICO , MJCO , MKCO ) 
VECTB(l)  =  MICO 
VECTB( 2)  =  MJCO 
VECTB( 3)  =  MKCO 

CALL  CPRODC VECTA, VECTB, MICO, MJCO, MKCO) 

*  W1  X  (W1  X  RA/G1) 

RAGl(l)  =  JX1  -  LCOGX(l) 

RAG1( 2)  =  JY1  -  LCOGY(l) 

RAG1(3)  =  JZ1  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA( 2)  =  W 1 ( 2 ) 

VECTA( 3)  =  Wl( 3) 

CALL  CPROD  (VECTA, RAG1.MIC1 ,MJC1,MKC1) 
VECTB ( 1 )  =  MIC1 
VECTB( 2)  =  MJC1 
VECTB(3)  =  MKC1 

CALL  CPROD  (VECTA, VECTB, MIC1 ,MJC1 ,MKC1) 

*  W2  X  (W2  X  RB/G2) 

RBG2( 1 )  =  JX1  -  LCOGX( 2) 

RBG2( 2)  =  JY1  -  LCOGY( 2) 

RBG2( 3)  =  JZ1  -  LCOGZ( 2) 

VECTA(l)  =  W2( 1) 

VECTA( 2)  =  W2( 2 ) 

VECTA( 3 )  =  W2( 3) 

CALL  CPROD  (VECTA, RBG2,MIC2,MJC2,MKC2) 
VECTB(l)  =  MIC2 
VECTB( 2)  =  MJC2 
VECTB( 3)  =  MKC2 
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CALL  CPROD  ( VECTA , VECTB , MIC2 , MJC2 , MKC2 ) 
W2  X  (W2  X  RA/G2) 

RAG2( 1)  =  JX2  -  LCOGX( 2) 

RAG2( 2)  =  JY2  -  LCOGY(2) 

RAG2( 3)  =  JZ2  -  LCOGZC2) 

VECTA(l)  =  W2( 1) 

VECTA( 2)  =  W2( 2) 

VECTA( 3)  =  W2( 3) 

CALL  CPROD  (VECTA, RAG2 ,MIC3 ,MJC3 ,MKC3) 
VECTB ( 1)  =  MIC3 
VECT6( 2)  =  MJC3 
VECTB(3)  =  MKC3 

CALL  CPROD( VECTA , VECTB , MIC3 , MJC3 , MKC3 ) 
W3  X  (W3  X  RB/G3) 

RBG3C1)  =  JX2  -  LCOGX(3) 

RBG3( 2)  =  JY2  -  LCOGY(3) 

RBG3( 3)  =  JZ2  -  LCOGZ( 3) 

VECTA(l)  =  W3( 1) 

VECTA( 2)  =  W3( 2) 

VECTA( 3 )  =  W3( 3 ) 

CALL  CPROD  ( VECTA , RBG3 , MIC4 , MJC4 , MKC4 ) 
VECTB ( 1)  =  MIC4 
VECTB  (’2)  =  MJC4 
VECTBC3)  =  MKC4 

CALL  CPROD  (VECTA, VECTB, MI C4,MJC4,MKC4) 
INERTIA  TRANSFORMATION 

TMAT( 2,1)  =  -DCOS  (  THZ  ) 

TMAT( 2,2)  =  -DSIN  (  THZ  ) 

TMAT( 2,3)  =  0.0D0 
DO  880  I  =  2,  3 

TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT (3,3)  =  DRCSZ(I) 

VECTA(l)  =  TMAT(2,1) 

VECTA( 2)  =  TMAT( 2,2) 

VECTA( 3)  =  TMAT( 2,3) 

VECTB ( 1 )  =  TMAT( 3,1) 

VECTB ( 2 )  =  TMAT( 3,2) 

VECTB( 3)  =  TMAT( 3,3) 

CALL  CPROD  (VECTA, VECTB, Mil, MJ1,MK1) 

TMAT( 1,1)  =  Mil 

TMAT( 1,2)  =  MJ1 

TMAT( 1,3)  =  MK1 

TMATTR( 1,1)  =  TMAT( 1,1) 

TMATTR( 1,2)  =  TMAT(2,1) 

TMATTR( 1,3)  =  TMAT(3,1) 

TMATTR( 2,1)  =  TMAT(1,2) 

TMATTR( 2,2)  =  TMAT(2,2) 

TMATTR( 2,3)  =  TMAT(3,2) 

TMATTR( 3,1)  =  TMAT(1,3) 

TMATTR(3,2)  =  TMAT(2,3) 

TMATTR( 3,3)  =  TMAT(3,3) 

DO  866  II  =  1,3 
DO  866  J  =1,3 
TEMP  =  0.  ODD 
DO  864  K  =1,3 
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TEMP  =  TMATTR(Il.K)  *  LIMAT(I,K 
864  CONTINUE 

MATTMP( I 1 , J)  =  TEMP 
866  CONTINUE 

DO  874  II  =  1,3 
DO  874  J  =  1,3 
TEMP  =  O.ODO 
DO  872  K  =  1,3 

TEMP  =  MATTMP( II ,K)  *  TMAT(K, J) 
872  CONTINUE 

NIMAT(Il.J)  =  TEMP 


874 

CONTINUE 

IXXT(I) 

= 

NIMAT(l.l) 

IXYT(I) 

= 

NIMAT( 1,2) 

IXZT(I) 

= 

NIMAT( 1,3) 

IYYT(I) 

= 

NIMAT( 2,2) 

IYZT(I) 

= 

NIMAT( 2,3) 

IZZT(I) 

= 

NIMAT( 3,3) 

880 

CONTINUE 

;V 

ENTER  CONSTANS 

INTO  MATRIX  A 

LINK  ONE 

?V 

SUM  OF  FORCES 

X  DIRECTION 

887 

MATA( 1,1) 

= 

1.  ODO 

MATA( 1,4) 

Ml 

MATA(l.lO) 

= 

-1.  ODO 

* 

Y  DIRECTION 

MATA( 2,2) 

= 

1.  ODO 

MATA( 2,5) 

= 

Ml 

MATAC2.il) 

= 

-1.  ODO 

* 

Z  DIRECTION 

MATA (3, 3) 

1.  ODO 

MATA( 3,6) 

= 

Ml 

MATA( 3,12) 

= 

-1.  ODO 

* 

EQUATIONS  AT 

JOINT  ZERO 

*.v 

X  DIRECTION 

MATA(4,4) 

= 

1.  ODO 

MATA( 4,8) 

= 

RBG1(3) 

MATA( 4,9) 

=  « 

-RBG1C2) 

Ve 

Y  DIRECTION 

MATA( 5,5) 

= 

1.  ODO 

MATA( 5,7) 

=  - 

-RBG1(3) 

MATA( 5,9) 

= 

RBGl(l) 

* 

Z  DIRECTION 

MATA(6,6) 

= 

1.  ODO 

MATA( 6,7) 

= 

RBG1( 2) 

MATA( 6,8) 

=  • 

-RBGl(l) 

* 

SUM  OF  MOMENTS 

EQUATIONS 

j- 

X  DIRECTION 

MATA( 7,2) 

= 

RBG1(3) 

MATA( 7,3) 

= 

-RBG1(2) 

MATA( 7,7) 

= 

-IXXT( 1) 

MATA( 7,8) 

= 

IXYT(l) 

MATA( 7,9) 

= 

IXZT(l) 

MATA( 7,11) 

= 

-RAG1(3) 

,J)  +  TEMP 


+  TEMP 


89 


MATA( 7,12) 

= 

RAG1( 2) 

* 

Y  DIRECTION 

MATA( 8,1) 

-RBG1(3) 

MATA( 8,3) 

= 

RBG1( 1) 

MATA( 8,7) 

= 

IXYT(l) 

MATA(8,8) 

= 

-IYYT(l) 

MATA( 8,9) 

— 

IYZT(l) 

MATA( 8,10) 

= 

RAG1(3) 

MATA(8, 12) 

= 

-RAG  1(1) 

* 

Z  DIRECTION 

MATA( 9,1) 

= 

RBG1( 2) 

MATA( 9,2) 

= 

-RBG1( 1) 

MATA( 9,7) 

= 

IXZT(l) 

MATA( 9,8) 

IYZT(l) 

MATA( 9,9) 
MATA(9 , 10) 

= 

-IZZT(l) 

= 

-RAG1(2) 

MATA( 9,11) 

= 

RAG  1(1) 

** 

LINK  TWO 

* 

SUM  OF  FORCES 

Vr 

X  DIRECTION 

939 

MATA( 10,10) 

= 

■  1.  ODO 

MATA( 10,13)  =  M2 


MATA( 10  j 19)  =  -1. 0D0 
Y  DIRECTION' 

MATA( 11,11)  =  1.  ODO 

MATA(11,14)  =  M2 
MATA( 11,20)  =  - 1. ODO 
Z  DIRECTION 

MATA( 12,12)  =  1.  ODO 

MATA( 12,15)  =  M2 
MATA( 12,21)  =  - 1. ODO 
EQUATIONS  AT  JOINT  ONE 
X  DIRECTION 


MATA( 13,4) 

= 

-1. ODO 

MATA( 13,8) 

= 

-RAG1(3) 

MATA( 13,9) 

= 

RAG1(2) 

MATA( 13,13) 

= 

1.  ODO 

MATA( 13,17) 

RBG2( 3) 

MATA( 13,18) 
DIRECTION 

= 

-RBG2( 2) 

MATA(14,5) 

= 

-1. ODO 

MATA( 14,7) 

= 

RAG1(3) 

MATA( 14,9) 

= 

-RAG1( 1) 

MATA( 14,141 

= 

1.  ODO 

MATA( 14 , 16; 

= 

-RBG2( 3) 

MATA( 14,18) 
DIRECTION 

= 

RBG2( 1) 

MATA( 15,6) 

- 

-1.  ODO 

MATA( 15,7) 

= 

-RAG1(2) 

MATA( 15,8) 

= 

RAG1( 1) 

MATA( 15,15) 

= 

1.  ODO 

MATA( 15,16) 

= 

RBG2( 2) 

MATA( 15,17) 

= 

-RBG2( 1) 

SUM  OF  MOMENTS  EQUATIONS 
X  DIRECTION 

MATA( 16,11)  =  RBG2( 3) 
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MATA( 16,12)  =  -RBG2( 2) 
MATA( 16,16)  =  -IXXT( 2) 
MATA( 16,17)  =  IXYT( 2) 
MATA( 16,18)  =  IXZT( 2) 
MATA( 16,20)  =  -RAG2( 3) 
MATA( 16,21)  =  RAG2( 2 ) 

*  Y  DIRECTION 

MATA( 17,10)  =  -RBG2( 3) 
MATA( 17,12)  =  RBG2( 1) 
MATA( 17,16)  =  IXYT( 2) 
MATA( 17,17)  =  -IYYT( 2) 
MATA( 17,18)  =  IYZT( 2) 
MATA( 17,19)  =  RAG2( 3) 
MATA( 17,21)  =  -RAG2( 1) 

*  Z  DIRECTION 

MATA( 18,10)  =  RBG2( 2) 
MATA( 18,11)  =  -RBG2( 1) 
MATA( 18,16)  =  IXZT( 2) 
MATA( 18,17)  =  IYZT( 2) 
MATA( 18,18)  =  -IZZT( 2) 
MATA( 18,19)  =  -RAG2(2) 
MATA( 18,20)  =  RAG2( 1 ) 
**  LINK  THREE 

*  SUM  OF  FORCES 
X  DIRECTION 

1000  MATA( 19,19)  =  1. ODO 

MATA( 19,22)  =  M3 

*  Y  DIRECTION 

MATA( 20,20)  «  1. ODO 

MATA( 20,23)  =  M3 

*  Z  DIRECTION 

MATA(21 ,21)  =  1. ODO 

MATA( 21,24)  =  M3 

*  EQUATIONS  AT  JOINT  TWO 

*  X  DIRECTION 

MATA(22, 13)  =  -1.  ODO 

MATA( 22,17)  =  -RAG2( 3) 
MATA( 22,18)  =  RAG2(2) 

MATA( 22,22)  =  l.ODO 
MATA( 22,26)  =  RBG3(3) 

MATA( 22,27)  =  -RBG3(2) 

*  Y  DIRECTION 

MATA( 23 , 14)  =  -l.ODO 
MATA(  23 , 16)  =  RAG2(3) 

MATA( 23,18)  =  -RAG2(1) 
MATA(23,23)  =  1.  CEO 

MATA( 23,25)  =  -RBG3(3) 
MATA( 23,27)  =  RBG3(1) 

*  Z  DIRECTION 

MATA( 24,15)  =  -l.ODO 
MATA( 24,16)  =  -RAG2(2) 
MATA( 24,17)  =  RAG2( 1) 
MATA(  24,24''  =  l.ODO 
MATA( 24,25;  =  RBG3(2) 
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MATAC 24, 26)  =  -RBG3(1) 

*  SUM  OF  MOMENTS  EQUATIONS 

*  X  DIRECTION 

MATA( 25,20)  =  RBG3(3) 

MATA(25 ,21)  =  -RBG3(2) 

MATA( 25,25)  =  -IXXT(3) 

MATA( 25,26)  =  IXYT(3) 

MATA( 25,27)  =  IXZT(3) 

*  Y  DIRECTION 

MATA( 26,19)  =  -RBG3(3) 

MATA( 26,21)  =  RBG3(1) 

MATAC 26,25)  =  IXYT(3) 

MATA( 26,26)  =  -IYYT(3) 

MATA( 26,27)  =  IYZT(3) 

*  Z  DIRECTION 

MATA( 27,19)  =  RBG3(2) 

MATA(27 ,20)  =  -RBG3(1) 

MATA( 27 , 25 )  =  IXZT(3) 

MATAC  27,26)  =  1YZT(3) 

MATA( 27,27)  =  -IZZT(3) 

*****  FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIRECTION  ***** 

1051  DO  1056  I  =  4,8 

DO  1055  J  =  1,27 
MATA( I, J)  =  0. ODO 

*  MATA( J, I)  =  0. ODO 

1055  CONTINUE 

1056  CONTINUE 

MATAC 4 ,4)  =  1. ODO 
MATAC 5, 5)  =  l.ODO 
MATAC  6, 6)  =  l.ODO 
MATAC  7, 7)  =  l.ODO 
MATAC 8, 8)  =  1. ODO 
DO  1065  J  =  1,27 

MATAC 18, J)  =  0. ODO 
MATAC 27, J)  =  0. ODO 
1065  CONTINUE 

IZZTC2)  =  IZZTC2)  +  M2  *  CDSQRTC LCOGXC 2 )**2+LC0GYC 2 )**2 ) )**2 
IZZTC3)  =  IZZT(3)  +  M3  *  C  DSQRT( LCOGXC  3)**2+LCOGYC  3)**2) )**2 
MATAC 9, 9)  =  -CIZZTC 1)+IZZT(2)+IZZTC3)) 

MATAC 18,9)  =  -l.ODO 

MATAC 18,18)  =  1.  ODO 

MATAC 27, 9)  =  -1. ODO 

MATAC  27,27)  =  l.ODO 

*  MULTIPLY  MATA  *  MATD  TO  OBTAIN  TORQUES 

DO  758  J  =  1,27 
SUM1  =  0.  ODO 
DO  755  K  =  1,27 

SUM1  =  SUM1  +  MATAC J,K)  *  MATD(K) 

755  CONTINUE 

MATC(J)  =  SUM1 
758  CONTINUE 

T2Z  =  -MATCC27) 

T2Y  =  -MATCC26) 

T2X  =  -MATCC25) 

T1Z  =  T2Z  -  MATCC18) 

T1Y  =  T2Y  -  MATC( 17) 


T1X  =  T2X  -  MATCC16) 

TOZ  =  T1Z  -  MATC ( 9 ) 

TOY  =  T1Y  -  MATC( 8 ) 

TOX  =  T1X  -  MATC( 7) 

*  ENTER  KNOWNS  INTO  MATB 

1077  MATB ( 1 )  =  0.  ODO 

MATB( 2)  =  0.  ODO 

MATB( 3)  =  -WG1 

MATB( 4)  =  AXO  -  MICO 

MATB( 5)  =  AYO  -  MJCO 

MATB( 6)  =  AZO  -  MKCO 

MATB( 7 )  =  T1X  -  TOX 

MATB( 8)  =  T1Y  -  TOY 

MATB( 9)  =  T1Z  -  TOZ 

MATB(IO)  =  0. ODO 

MATB(ll)  =  0. ODO 

MATB(12)  =  -WG2 
MATBC13)  =  MIC1  -  MIC2 

MATB (14)  =  MJC1  -  MJC2 

MATB( 15 )  =  MKC1  -  MKC2 

MATB( 16)  =  T2X  -  T1X 

MATB( 17)  =  T2Y  -  T1Y 

MATB( 18)  =  T2Z  -  T1Z 

MATB( 19)  =  0.  ODO 

MATB (20)  =  0.  ODO 

MATB( 21 )  =  -WG3 

MATB( 22)  =  MIC3  -  MIC4 

MATB( 23)  =  MJC3  -  MJC4 

MATB ( 24)  =  MKC3  -  MKC4 

MATB( 25)  =  -T2X 

MATB( 26)  =  -T2Y 

MATB(27)  =  -T2Z 

*****  FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIRECTION  ***** 

DO  1107  I  =  4,8 
MATB ( I )  =  0. ODO 

1107  CONTINUE 

MATB( 18)  =  0.  ODO 

MATB( 27)  =  0. ODO 

*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

1112  CALL  L£QT2F(MATA,M,N, IA, MATB, IDGT,WKAREA, IER) 

IF  ( IER. EQ. 0)  THEN 

GOTO  1133 

ELSE 

WRITE  (*,1117)  IER 

1117  FORMAT  (15) 

DO  1127  I  =  1,  27 
WRITE  (*,1120)  I 

1120  FORMAT  (17) 

DO  1124  J  =  1,  27,  3 

WRITE  (*,1123)  J,MATA(I,J) , J+l ,MATA( I , J+l) , J+2,MATA( I , J+2) 

1123  FORMAT  ( 15 ,F13. 5 , 15 ,F13. 5 ,15 ,F13. 5) 

1124  CONTINUE 

WRITE  (*,1126)  I ,MATB( I) 

1126  FORMAT  (I3.F15.5) 

1127  CONTINUE 


CALL  ENDJOB 
END  IF 

***  FIND  LCOGX , LCOGY , LCOGZ , THETA  VALUE S,WX,WY,WZ 

*  JOINT  ZERO 

1133  FXO  =  MATE ( 1 ) 

FYO  =  MATB ( 2 ) 

FZO  =  MATB ( 3 ) 

*  LINK  ONE 

*  SINCE  LINK1  IS  CONSTRAIN  TO  ROTATE  AROUND  THE  Z  ONLY 

AX1  =  0.  0D0 
AY1  =  0.  ODO 
AZ1  =  0. ODO 


*141 

AX1 

=  MATB(4) 

* 

VELX1 

=  INTGRLCO.  . AX1) 

* 

LC0GX1 

=  INTGRL(X1 ,VELX1) 

* 

LCOGX(l) 

=  LC0GX1 

* 

AY1 

=  MATB(5) 

* 

VELY1 

=  INTGRLCO.  ,AY1) 

*> 

LCOGY 1 

=  INTGRL( Y1 , VELY1) 

* 

LCOGY(l) 

=  LCOGY 1 

* 

AZ1 

=  MATB ( 6 ) 

Vf 

VELZ1 

=  INTGRLCO. , AZl) 

V? 

LCOGZ 1 

=  INTGRLC  Z1 ,VELZ1) 

* 

LCOGZ(l) 

=  LCOGZ 1 

WD1X 

=  MATB  C  7 ) 

W1X 

=  INTGRLCO. ,WD1X) 

WDX(l) 

=  WD1X 

Wl(l) 

=  W1X 

WD1Y 

=  MATB( 8) 

W1Y 

=  INTGRLCO.  ,WD1Y) 

WDY( 1 ) 

=  WD1Y 

W 1  ( 2 ) 

=  W'lY 

WD1Z 

=  MATBC9) 

W1Z 

=  INTGRLC 2.  ,WD1Z) 

WDZ(l) 

=  WD1Z 

Wl(3) 

=  VIZ 

1166 

THZ 

=  INTGRLCO. ,W1Z) 

THZANG 

=  THZ  * **  RADEG 

COSTHZ 

=  DCOSCTHZ) 

SINTHZ 

=  DSINCTHZ) 

*  IF  THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 

*  THE  DIRECTION  COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 

*  AND  DO  NOT  NEED  TO  BE  CALCULATED 

*  JOINT  ONE 

1174  FX1  =  MATB(IO) 

FY1  =  MATB(ll) 

FZ1  =  MATB( 12) 

**  LINK  TWO 

1178  AX 2  =  MATB( 13) 

VELX2  =  INTGRLC-O. 416.AX2) 

LC0GX2  =  INTGRL(X2 ,VELX2) 

LC0GX(2)  =  LC0GX2 
AY2  =  MATB( 14) 

VELY2  =  INTGRLCO. ,AY2) 

LC0GY2  =  INTGRLC Y2,VELY2) 

LCOGY( 2)  =  LC0GY2 
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AZ2 

= 

MATBC15) 

VELZ2 

= 

INTGRLC 0.  416, AZ2) 

LC0GZ2 

= 

INTGRLC  Z2 , VELZ2 ) 

LCOGZC 2) 

= 

LC0GZ2 

WD2X 

= 

MATBC16) 

W2X 

= 

INTGRLC 2.  ,WD2X) 

WDX( 2) 

= 

WD2X 

W2C1) 

= 

W2X 

WD2Y 

= 

MATBC17) 

W2Y 

= 

INTGRLC 0.  ,WD2Y) 

WDYC2) 

= 

WD2Y 

W2C  2) 

= 

W2Y 

WD2Z 

= 

MATBC18) 

W2Z 

= 

INTGRLC 2.  ,WD2Z) 

W2C3) 

r* 

W2Z 

WDZC2) 

= 

WD2Z 

*  GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 

DRCSX(2)  =  ( LCOGX2  -  JX1)  /  LNCOG2 

DRCSY( 2)  =  ( LCOGY2  -  JY1)  /  LNCOG2 

DRCSZ( 2)  =  ( LCOGZ2  -  JZ1)  /  LNCOG2 

AMP  =  DSQRT(DRCSX(2)* **DRCSX(2)+DRCSY(2)*DRCSY(2)+.  . 

DRCSZ( 2 )*DRCSZ( 2 ) ) 

DRCSX(2)  =  DRCSX( 2) /AMP 

DRCSY( 2)  =  DRCSY( 2) /AMP 

DRCSZ( 2)  =  DRCSZ( 2) /AMP 

DRCANXC 2 )  =  DACOS(DRCSX( 2) )  *  RADEG 

DRCANYC  2)  =  DACOS(DRCSY( 2) )  *  RADEG 

DRCANZ(2)  =  DACOS(DRCSZ( 2) )  *  RADEG 

*  JOINT  LOCATION 

1215  JX2  =  JX1  +  LINKL2  *  DRCSX(2) 

JY2  *  JY1  +  LINKL2  *  DRCSY(2) 

JZ2  =  JZ1  +  LINKL2  *  DRCSZ(2) 

*  JOINT  WO 

1220  FX2  =  MATB(19) 

FY2  =  MATBC20) 

FZ2  =  MATB(21) 

**  LINK  THREE 

1224  AX3  =  MATB( 22) 

VELX3  =  INTGRLC -1. 282, AX3) 

LCOGX3  =  INTGRLC X3 , VELX3 ) 

LCOGX( 3)  =  LCOGX3 

AY3  =  MATBC23) 

VELY3  =  INTGRLC 0. ,AY3) 

LC0GY3  =  INTGRLC Y3.VELY3) 

LCOGYC  3)  =  LC0GY3 

AZ3  =  MATBC24) 

VELZ3  =  INTGRLC 1. 282, AZ3) 

LC0GZ3  =  INTGRLC Z3.VELZ3) 

LCOGZC  3)  =  LC0GZ3 

WD3X  =  MATBC25) 

W3X  =  INTGRLC 2. ,WD3X) 

WDXC3)  =  WD3X 

W3C  1 )  =  W3X 

WD3Y  =  MATB(26) 

W3Y  =  INTGRLC 0.  ,WD3Y) 


WDY ( 3 )  =  WD3Y 

W  3 ( 2)  =  W3Y 

WD3Z  =  MATB( 27 ) 

W'3Z  =  INTGRL( 2.  ,WD3Z) 

WDZ( 3)  =  WD3Z 

W3( 3)  =  W3Z 

*  CALC  DIRECTIONAL  COSINES  FOR  LINK  THREE 
1249  DRCSXC 3)  =  (LCOGX3  -  JX2)  /  LNCOG3 

DRCSY( 3)  =  ( LCOGY3  -  JY2)  /  LNCOG3 
DRCSZ( 3)  =  ( LCOGZ3  -  JZ2)  /  LNCOG3 
AMP  =  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+. .  . 

DRCSZ( 3)*DRCSZ(3) ) 

DRCSXC3)  =  DRCSX(3)/AMP 

DRCSY( 3)  =  DRCSY(3)/AMP 

DRCSZ( 3)  =  DRCSZ( 3) /AMP 

DRCANXC  3)  =  DACOS( DRCSX( 3) )  *  RADEG 

DRCANYC  3)  =  DACOS( DRCSY( 3) )  *  RADEG 

DRCANZC 3)  =  DACOS(DRCSZ( 3) )  *  RADEG 

*  TIP  LOCATION 

1261  TIPX  =  JX2  +  LINKL3  *  DRCSX(3) 

TIPY  =  JY2  +  LINKL3  *  DRCSY(3) 

TIPZ  =  JZ2  +  LINKL3  *  DRCSZ(3) 

*  FIND  THE  ANGLE  BETWEEN  THE  LIMKS 
1265  ANG23X  =  DRCANX(2)  -  DRCANX(3) 

ANG23Y  =  DRCANYC  2 )  -  DRCANY( 3) 

ANG23Z  =  DFCANZ( 2)  -  DRCANZ(3) 

ANG12X  =  DRCANX(l)  -  DRCANXC  2 ) 

ANG12Y  =  DRCANY(l)  -  DRCANY( 2) 

ANG12Z  =  DRCANZC 1)  -  DRCANZC 2) 

ANG12  =  DRCANZC 2)  -  DRCANZC 1) 

ANG23  =  ANG23X  +  ANG23Y  +  ANG23Z 

*  CLACULATE  THE  ERROR  BETWEEN  KNOWN  TIP  POSITION  AND  CALCULATED 

ERROR1  =  ABS  C  LTIPX  -  TIPX  )  /  0. 867  *  100. 0 
ERR0R2  =  ABS  (  LTIPY  -  TIPY  )  /  0.867  *  1G0. 0 
ERR0R3  =  ABS  C  LTIPZ  -  TIPZ  )  /  0.867  *  100.0 

END 

STOP 

FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPRODC  VECTA , VECTB , MI , MJ , MK) 

1284  IMPLICIT  REAL* 8  CA-Z) 

DIMENSION  VECTAC  3) , VECTBC  3) 

MI  =  VECTAC 2)  *  VECTBC 3)  -  VECTAC 3)  *  VECTBC 2) 

MJ  =  VECTAC 3)  *  VECTBC 1)  -  VECTAC 1)  *  VECTBC 3) 

MK  =  VECTAC 1)  *  VECTBC 2)  -  VECTAC 2)  *  VECTBC 1) 

RETURN 

END 
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APPENDIX  D 


ROBOT  VALIDATION  PROGRAM 


TITLE  ROBOT  TORQUE  VALIDATION  PROGRAM 


* 

* 


* 


* 

* 


THIS  IS  A  PROGRAM  THAT  USES  ACTUAL  DATA  FROM  THE  NEPTUNE  II  ROBOT  TO  * 
OBTAIN  TORQUES  TO  INPUT  INTO  THE  NONSINGULAR  SIMULATION  OF  A  3  LINK  * 
RIGID  MANIPULATOR.  THE  ORIGINAL  PROGRAM  WAS  WRITTEN  BY  LT  ALTINOK  * 
AND  HAS  BEEN  GREATLY  MODIFIED  TO  INCLUDE  3  D  MOTION  AND  GRAVITATIONAL* 
EFFECTS  BY  LT  R.  M.  VERBOS  USN  SEPTEMBER  1988  * 


jl  ^  j-  ju  -'r  Vr  Vr  ^  Vr  Vr  Vr  Vr  *'?  Vr  “VVr  Vr  Vr  Vr  Vr  Vr 1 


TERMINAL 

METHOD  RKSFX 

PRINT  . 10 ,DRCANZ( 3) 

CONTROL  FINTIM  =7.0,  DELT  =  .01,  DELMAX  =  .  1,  DELPRT  =  .  10 
SAVE  .10.  DRCANZ( 3) 

GRAPH  ( DE=TEK618)  TIME  ,DRCA.NZ(  3) 

D  DIMENSION  MATA( 27 , 27) ,MASS( 3 ,2) ,L( 3 ,2) ,RX( 3 ,2) ,RY( 3 , 2) ,RZ( 3 ,2) 

D  DIMENSION  IXX( 3 , 2) , IXZ( 3 ,2) , IXY( 3 , 2) , IYY( 3 ,2) , IYZ( 3 , 2) , IZZ( 3 , 2) 

D  DIMENSION  IMAT(3,3,3) ,NIMAT(3,3) ,TMAT(3,3) ,TMATTR( 3,3) ,MATTMP(3,3) 

D  DIMENSION  LIMAT(3,3,3) 

FIXED  IER , I , J ,M,K ,P ,N, IA, IDGT, A, II 
ARRAY  MATBC27) .LCOGXC 3) , LCOGY( 3) ,LCOGZ(3) 

ARRAY  VECTA0(  3 ) , VECTBO( 3 ) , VECTA 1 ( 3 ) , VECTB 1(3), VECTA2 ( 3 ) , VECTB2 ( 3 ) 

ARRAY  VECTA ( 3) , VECTB( 3) 

ARRAY  WDX ( 3 ) ,WDY(3) .VDZ(3) ,W1(3) ,V2(3) ,W3(3) 

ARRAY  RBG1(3) ,RAG1( 2) ,RBG2( 3) ,RAG2( 3) ,RBG3( 3) 

ARRAY  WKAREA( 850) 

ARRAY  IXXT(3),IYYT(3),IZZT(3) , IXYT( 3) , IXZT( 3) , IYZT( 3) 

ARRAY  DRCANXC  3) , DRCANY ( 3 ) ,DRCANZ( 3) 

ARRAY  DRCSX( 3) ,DRCSY( 3) ,DRCSZ( 3) 

INITIAL 

*****  j  NPUT  ***** 

*  INPUT  PARAMETER  CONSTANTS 

IDGT  =  6 
G  =  9. 8  IDO 
N  =  27 
M  =  1 
IA  =  27 
IER  =  0 
LEVELQ  =  0 
LEVLDQ  =  0 


* 

* 


INPUT  DISTANCE  FROM  COG  OF  LINK  TO  CENTER  OF  MASS 
FOR  EACH  LINK  ENDS 


L(  1,1)  =  0. 1953DO 
L(  1 , 2)  =  0. 065 1D0 
L(  2 , 1)  =  0.208D0 
L(2,2)  =  0. 208D0 
L(  3 , 1)  =  0.  1746D0 

*  L( 3 , 2)  =  0. 1746D0 

*  INPUT  THE  LINK  LENGTHS  OF  THE  ROBOT 

LINKL1  =  0. 2604D0 
LINKL2  =  0.416D0 
LINKL3  =  0. 3498D0 


*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  0.  0D0 
JYO  =  0. ODO 
JZO  =  0. ODO 
JX1  =  0. ODO 
JY1  =  0. ODO 
JZ1  =  0. 2604D0 
JX2  =  0. ODO 
JY2  =  0.416D0 
JZ2  =  0. 2604D0 

*  INPUT  TORQUE  CONSTANTS 

TOX  =  0. ODO 

TOY  =  0. ODO 

TOZ  =  0. ODO 

T1X  =  0.  ODO 

T1Y  =  0. ODO 

T1Z  =  0. ODO 

T2X  =  0. ODO 

T2Y  =  0. ODO 

T2Z  =  0. ODO 

TGI  =  0.  ODO 

TG2  =  0.  ODO 

T1FNC  =  0.  ODO 

T2FNC  =  0. ODO 

*  INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 

110  MASS( 1, 1)  =  2. 273D0 

MASS( 1,2)  =  6. 818D0 
MASS ( 2 , 1)  =  0.  455D0 
MASS( 2,2)  =  0.455D0 
MASS( 3,1)  =  5.  909D0 
MASS( 3,2)  =  5.909D0 

*  INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 

120  LCOGX(l)  =  0.  ODO 

XI  =  LCOGX(l) 

LCOGY(l)  =  0. ODO 

Y1  =  LCOGY(l) 

LCOGZ(l)  =  0. 10D0 

Z1  =  LCOGZ(l) 

LC0GX(2)  =  0. ODO 

X2  =  LCOGX( 2) 

LC0GY(2)  =  0.  208D0 
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Y2  =  LC0GY(2) 

LCOGZ( 2)  =  0.2604D0 

Z2  =  LCOGZ( 2) 

LCOGX( 3)  =  O.ODO 
X3  =  LCOGX(3) 

LCOGY(3)  =  0.4159D0 

Y3  =  LCOGY( 3) 

LCOGZ( 3)  =  0.  0858D0 

Z3  =  LCOGZ( 3) 

*  INPUT  MASS  OF  EACH  LINK  IN  KG 

Ml  =  9.O91D0 
M2  =  0. 910D0 
M3  =  11.818D0 

*  INPUT  ACCELERATIONS  OF  JOINT  ZERO 

AXO  =  0. ODO 
AYO  =  O.ODO 
AZO  =  O.ODO 

*  INPUT  THE  INITIAL  DIRECTION  COSINES 

DRCSX( 1)  =  0.  ODO 
DRCSY( 1)  =  0. ODO 
DRCSZ(l)  =  1.  ODO 
DRCSX( 2)  =  0.  ODO 
DRCSY( 2)  =  1.  ODO 
DRCSZ(2)  =  O.ODO 
DRCSX( 3)  =  0.  ODO 
DRCSY( 3)  =  O.ODO 
DRCSZ( 3)  =  -1. ODO 

*  INPUT  THE  INITIAL  DIRECTION  COSINE  ANGLES 

DRCANX(l)  =  90. ODO 
DRCANY(l)  =  90. ODO 
DRCANZ(l)  =  O.ODO 
DRCANX( 2)  =  90. ODO 
DRCANY( 2)  =  O.ODO 
DRCANZ( 2)  =  90. ODO 
DRCANX( 3 )  =  90. ODO 
DRCANYC  3)  =  90. ODO 
DRCANZ(3)  =  180. ODO 

*****  INITIALIZE  ***** 

*  OMEGA  AND  OMEGA  DOT 

160  DO  170  I  =  1,3 

W1(I)  =  O.ODO 

W2(I)  =  O.ODO 

W3(  I )  =  O.ODO 

WDX(  I )  =0.  ODO 

WDY(I)  =  O.ODO 

WDZ(I)  =  O.ODO 

170  CONTINUE 

W3( 1)  =  0. 158D0 
W3IC  =  W3( 1) 

THZ  =  0,  ODO 


99 


*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  180  I  =  1,27 
DO  175  J  =  1,27 

MATA(I.J)  =  0.0D0 
175  CONTINUE 

MATB(I)  =  0.  0D0 
180  CONTINUE 

*  CONSTANT  TO  CONVERT  STRIP  DATA  TO  DELTA  P 

RDVTOV  =  0. 2D0 
RAREA  =  0. 00312D0 
RLA  =  0.0597/2.0 
RVTOP  =  10. ODO 
RLBTON  =  4. 448D0 
R INTOM  =  39. 37D0 

RCNFAC  =  RDVTOV*RVTOP*RAREA*RLA*RLBTON*R I NT0M**2 
*****  CALCULATIONS  ***** 


*  WEIGHTS  (NEWTONS) 

185  WG1  =  M1*G 

WG2  =  M2*G 
WG3  =  M3*G 


*  COMPUTE  THE  LENGTH  FROM  THE  INBOARD  JOINT  TO  COG 

LNC0G1  =  DSQRT  (  LCOGX( 1)*LC0GX( 1)  +  LCOGY( 1 )*LCOGY( 1 )  +.  .  . 

LCOGZ( 1)*LC0GZ( 1)  ) 

LX 2  =  LC0GX(2)  -  JX1 

LY2  =  LC0GY( 2)  -  JY1 

LZ2  =  LC0GZC2)  -  JZ1 

LNC0G2  =  DSQRT  (  LX2*LX2  +  LY2*LY2  +  LZ2*LZ2  ) 

LX 3  =  LCOGX( 3)  -  JX2 

LY3  =  LCOGY( 3)  -  JY2 

LZ3  =  LC0GZC3)  -  JZ2 

LNC0G3  =  DSQRT  (  LX3*LX3  +  LY3*LY3  +  LZ3*LZ3  ) 

V3IC  =  LNC0G3  *  W3( 1) 

*  L( 3 , 2) 

L(3,2)  =  (13.5  *  RCNFAC)  /  (  MASS(3,2)  *  G)  -  LNC0G3 

*  COMPUTE  INITIAL  INERTIAS  BASED  ON  POINT  MASSES 

*  IN  GLOBAL  COORDINATES 

190  DO  225  I  =  1,3 

RX( 1,1)  =  -L( I , 1 )  *  DRCSX(I) 

RX( 1,2)  =  L( I ,2)  *  DRCSX(I) 

RY( I , 1)  =  - L( 1,1)  *  DRCSY(I) 

RY( 1,2)  =  L( 1,2)  *  DRCSY(I) 

RZ( 1,1)  =  -L( 1,1)  *  DRCSZ(I) 

RZ( 1,2)  =  L( I ,2)  *  DRCSZ(I) 

200  IXX(I,1)  =  MASS( 1,1)  *  ( ( RY( 1,1)  *  RY(I,1))  +  (RZ(I,1)  *  RZ(I,1))) 

IXX( 1,2)  =  MASS( 1,2)  *  ((RY( I ,2)  *  RY(I,2))  +  (RZ(I,2)  *  RZ(I,2))) 

lYYIYM  =  TYYCT  1  +  TYYf  T  9  'i 

IYY( 1,1)  =  MASS( 1,1)  *  ( (RX( 1,1)  *  RX(I,1))  +  (RZ(I,1)  *  RZ(I,1))) 

IYY( 1,2)  =  MASS( 1,2)  *  ((RX(I,2)  *  RX(I,2))  +  ( RZ( 1,2)  *  RZ(I,2))) 

IYYT(I)  =  IYY( 1,1)  +  IYY( 1,2) 
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IZZ(I.l)  =  MASS(I,1)  *  ( ( RX( 1,1)  *  RX( 1,1))  +  (RY(I,1)  *  RY(I,1))> 
IZZ(I,2)  =  MASS( 1,2)  *  ( (RX( 1,2)  *  RX(I,2))  +  (RY(I,2)  *  RY(I,2))) 
IZZT(I)  =  IZZ(I,1)  +  IZZ(I,2) 

205  IXY( 1,1)  =  MASS(I,1)  *  RX(I,1)  *  RY(I,1) 

IXY( 1,2)  =  MASS( 1,2)  *  RX( 1,2)  *  RY(I,2) 

IXYT(I)  =  IXY(I.l)  +  IXY( 1,2) 

IXZ(I.l)  =  MASS(I,1)  *  RZ( 1,1)  *  RX( 1,1) 

IXZCI.2)  =  MASS( 1,2)  *  RZ( 1,2)  *  RX(I,2) 

IXZT(I)  =  IXZ( 1,1)  +  IXZ( 1,2) 

IYZ(I.l)  =  MASS(I,1)  *  RY( I , i)  *  RZ( 1,1) 

IYZ( 1,2)  =  MASS( 1,2)  *  RY( 1,2)  *  RZ(I,2) 

IYZT(I)  =  IYZ( 1,1)  +  IYZ( 1,2) 

IF  (IXXT(I)  . LE.  . 10)  THEN 

IXXT(I)  =  . 10 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 

IF  (IYYT(I)  .LE.  .01)  THEN 

IYYT(I)  =  .01 

ELSE 

IYYT(I)  =  IYYT(I) 

END  IF 

IF  (IZZT(I)  -LE.  .01)  THEN 

IZZT(I)  =  .01 

ELSE 

IZZT(I)  =  IZZT(I) 

END  IF 

IMAT( 1,1,1)  =  IXXT(I) 

IMAT( 1,1,2)  =  IXYT(I) 

IMAT( 1,1,3)  =  IXZT(I) 

IMAT( 1,2,1)  =  -IXYT(I) 

IMAT( 1,2,2)  =  IYYT(I) 

IMAT( 1,2,3)  =  IYZT(I) 

IMAT( 1,3,1)  =  -IXZT(I) 

IMAT( 1,3,2)  =  -IYZT(I) 

IMAT( 1,3,3)  =  IZZT(I) 

225  CONTINUE 

*  DUE  TO  LINK  1  CONSTRAINTS  LINK  1  INERTIAS  ARE  CONSTANT 

IXXT(l)  =  IMAT( 1,1,1) 

IXYT(l)  =  JMAT( 1,1,2) 

IXZT(l)  =  IMAT( 1,1,3) 

IYYT(l)  =  IMAT( 1,2,2) 

IYZT(l)  =  IMAT( 1,2,3) 

IZZT(l)  =  IMAT( 1,3,3) 

*  TRANSFORM  THE  INITIAL  INERTIAS  TO  LOCAL  COORDINATED 

DO  9  I  =  2,  3 

TMAT( 2,1)  =  -DCOS  (  THZ  ) 

TMAT(2,2)  =  -DSIN  (  THZ  ) 

TMAT( 2,3)  =  0.  ODO 
TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT( 3,3)  =  DRCSZ(I) 
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VECTA(l)  =  TMAT( 2,1) 

VECTAC2)  =  TMAT( 2,2) 

VECTA( 3)  =  TMAT( 2,3) 

VECTB(l)  =  TMAT( 3,1) 

VECTB( 2)  =  TMAT( 3,2) 

VECTB( 3)  =  TMAT( 3,3) 

CALL  CPROD  ( VECTA,VECTB ,MI1 ,MJ1 ,MK1) 

TMAT(1,1)  =  Mil 
TMAT( 1,2)  =  MJ1 
TMAT( 1,3)  =  MK1 
TMATTR(l.l)  =  TMAT(1,1) 

TMATTR( 1,2)  =  TMAT(2,1) 

TMATTR( 1,3)  =  TMAT(3,1) 

TMATTR( 2,1)  =  TMAT( 1,2) 

TMATTR(2,2)  =  TMAT(2,2) 

TMATTR(2,3)  =  TMAT(3,2) 

TMATTR( 3,1)  =  TMAT(1,3) 

TMATTR( 3,2)  =  TMAT(2,3) 

TMATTR(3,3)  =  TMAT(3,3) 

DO  5  II  =  1,3 
DO  5  J  =1,3 
TEMP  =  0. 0D0 
DO  1  K  =1,3 

TEMP  =  TMAT(I1,K)  *  IMAT(I,K,J)  +  TEMP 

1  CONTINUE 

MATTMP( I 1 , J)  =  TEMP 

5  CONTINUE 

DO  8  II  =  1,3 
DO  8  J  =  1,3 
TEMP  =  O.ODO 
DO  7  K  =  1,3 

TEMP  =  MATTMP( I 1 ,K)  *  TMATTR(K , J)  +  TEMP 

7  CONTINUE 

LIMAT( I , I 1 , J)  =  TEMP 

8  CONTINUE 

9  CONTINUE 

DERIVATIVE 

NOSORT 

230  CALL  ERRSET  (208,256,-1,1,1) 

CALL  UERSET( LEVELQ , LEVLDQ) 


*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  240  I  =  1,27 
DO  235  J  =  1,27 

MATA( I , J)  =  O.ODO 
235  CONTINUE 

MATB(I)  =  0.  ODO 
240  CONTINUE 

*  INPUT  JOINT  EQUATIONS 
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*  JOINT  ZERO  EQUATIONS 

*  W1  X  (W1  X  RB/G1 ) 

RBG1(1)  =  JXO  -  LCOGX(l) 

RBG1( 2)  =  JYO  -  LCOGY(l) 

RBG1( 3)  =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA(2)  =  Wl( 2) 

VECTA( 3 )  =  Wl( 3) 

CALL  CPROD( VECTAO , RBG 1 , MI CO , M JCO , MKCO ) 
VECTBO(l)  =  MICO 
VECTBO( 2)  =  MJCO 
VECTBO( 3)  =  MKCO 

C ALL  C PROD ( VE  CTA  0 , VE  CTB  0 , M I C  0 , M JC 0 , MKC  0 ) 

*  INPUT  JOINT  EQUATIONS 

*  JOINT  ZERO  EQUATIONS 

*  W1  X  (W1  X  RB/G1) 

RBG1( 1 )  =  JXO  -  LCOGX(l) 

RBG1(2)  =  JYO  -  LCOGY(l) 

RBG1( 3)  =  JZO  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA( 2)  =  W 1 ( 2 ) 

VECTA(3)  =  V 1 ( 3 ) 

CALL  CPROD(VECTA,RBGl, MICO, MJCO, MKCO) 
VECTB(l)  =  MICO 
VECTB( 2)  =  MJCO 
VECTB( 3)  =  MKCO 

CALL  CPROD(VECTA,VECTB, MICO, MJCO, MKCO) 

*  W1  X  (W1  X  RA/G1) 

RAGl(l)  =  JX1  -  LCOGX(l) 

RAG1(2)  =  JY1  -  LCOGY(l) 

RAG1(3)  =  JZ1  -  LCOGZ(l) 

VECTA(l)  =  Wl(l) 

VECTA( 2)  =  W 1 ( 2 ) 

VECTA(3)  =  W 1 ( 3 ) 

CALL  CPROD  ( VECTA ,RAG1 ,MIC1 ,MJC1 ,MKC1) 
VECTB(l)  =  MIC1 
VECTB( 2)  =  MJC1 
VECTB( 3)  =  MKC 1 

CALL  CPROD  (VECTA, VECTB ,MIC1 ,MJC1 ,MKC1 ) 

*  W2  X  (W2  X  RB/G2) 

RBG2( 1)  =  JX1  -  LCOGXC  2) 

RBG2( 2)  =  JY1  -  LCOGY( 2) 

RBG2( 3)  =  JZ1  -  LCOGZC2) 

VECTA(l)  =  W2( 1) 

VECTA(2)  =  W2( 2 ) 

VECTAO)  =  W2(  3) 

CALL  CPROD  ( VECTA, RBG2 ,MIC2,MJC2,MKC2) 
VECTB ( 1 )  =  MIC2 
VECTB(2)  =  MJC2 
VECTB(3)  =  MKC 2 

CALL  CPROD  (VECTA, VECTB, MIC2 ,MJC2 ,MKC2) 

*  W2  X  ( W2  X  RA/G2) 

RAG2(1)  =  JX2  -  LCOGX( 2 ) 

RAG2( 2)  =  JY2  -  LCOGY(2) 

RAG2( 3)  =  JZ2  -  LCOGZ( 2) 
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VECTA(l)  =  W2( 1 ) 

VECTA( 2)  =  W2( 2) 

VECTA( 3)  =  W2( 3) 

CALL  CPROD  ( VECTA ,RAG2 ,MIC3 ,MJC3 ,MKC3) 

VECTB(l)  =  MIC3 
VECTBC2)  =  MJC3 
VECTB(3)  =  MKC3 

CALL  CPROD( VECTA , VECTB , MIC3 , MJC3 , MKC3 ) 

*  W3  X  (W3  X  RB/G3) 

RBG3( 1)  =  JX2  -  LCOGX( 3) 

RBG3( 2)  =  JY2  -  LC0GY(3) 

RBG3( 3)  =  JZ2  -  LCOGZ(3) 

VECTA(l)  =  W3( 1 ) 

VECTA( 2)  =  W3( 2) 

VECTA( 3 )  =  W3( 3) 

CALL  CPROD  (VECTA , RBG3 ,MIC4 ,MJC4 ,MKC4) 

VECTB(l)  =  MIC4 
VECTB(2)  =  MJC4 
VECTB( 3)  =  MKC4 

CALL  CPROD  ( VECTA , VECTB ,MIC4 ,MJC4 ,MKC4) 

*  INERTIA  TRANSFORMATION 

DO  390  1=2,  3 

TMAT( 2,1)  =  -DCOS  (  THZ  ) 

TMATf  2,2)  =  -DSIN  (  THZ  ) 

TMAT( 2,3)  =  0.  ODO 
TMAT( 3,1)  =  DRCSX(I) 

TMAT( 3,2)  =  DRCSY(I) 

TMAT( 3,3}  =  DRCSZ(I) 

VECTA(l)  =  TMAT( 2,1) 

VECTA( 2)  =  TMAT( 2,2) 

VECTA( 3)  =  TMAT( 2,3) 

VECTB(l)  =  TMAT(3,1) 

VECTB(2)  =  TMAT( 3,2) 

VECTB(3)  =  TMAT( 3,3) 

CALL  CPROD  (VECTA , VECTB ,MI1,MJ1,MK1) 

TMAT( 1,1)  =  Mil 
TMAT( 1,2)  =  MJ1 
TMAT( 1,3)  =  MK1 
TMATTR( 1,1)  =  TMAT(l.l) 

TMATTR( 1,2)  =  TNAT(2,1) 

TMATTR( 1,3)  =  TMAT(3,I) 

TMATTR( 2,1)  =  TMAT(1,2) 

TMATTR(2,2)  =  TMAT(2,2) 

TMATTR( 2,3)  =  TMAT(3,2) 

TMATTR( 3,1)  =  TMAT(1,3) 

TMATTR( 3, 2)  =  TMAT(2,3) 

TMATTR(3,3)  =  TMAT(3,3) 

DO  375  II  =  1,3 
DO  375  J  =1,3 
TEMP  =  0.  ODO 
DO  370  K  =1,3 

TEMP  =  TMATTR( II ,K)  *  LIMAT(I,K,J)  +  TEMP 

370  CONTINUE 
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MATTMP(Il.J)  =  TEMP 
375  CONTINUE 

DO  380  II  =  1,3 
DO  380  J  =1,3 
TEMP  =  0. 0D0 
DO  378  K  =  1,3 

TEMP  =  MATTMP(I1,K)  *  TMAT(K, J)  +  TEMP 
378  CONTINUE 

NIMAT( 1 1 , J)  =  TEMP 
380  CONTINUE 

IXXT(I)  =  NIMAT(l.l) 

IXYT(I)  =  NIMAT( 1,2) 

IXZT(I)  =  NIMAT( 1,3) 

IYYT(I)  =  NIMAT( 2,2) 

IYZT(I)  =  NIMAT( 2,3) 

IZZT(I)  =  NIMAT( 3,3) 

390  CONTINUE 

*****  ENTER  CONSTANTS  INTO  MATRIX  A  ***** 

**  LINK  ONE 


SUM  OF  FORCES 


Vc 

X 

DIRECTION 

395 

MATA( 1,1) 

1.  ODO 

MATA( 1,4) 

= 

Ml 

MATA( 1,10) 

= 

-1.  ODO 

MATB(l) 

= 

0.  ODO 

* 

Y 

DIRECTION 

MATA( 2,2) 

1.  ODO 

MATA (2, 5) 

= 

Ml 

MATA( 2,11) 

= 

-1.  ODO 

MATB( 2) 

= 

0.  ODO 

* 

Z 

DIRECTION 

MAT A (3,3) 

= 

1.  ODO 

MATA( 3,6) 

Ml 

MATA( 3,12) 

-1.  ODO 

MATB(3) 

= 

-VG1 

EQUATIONS  AT  JOINT  ZERO 


X  DIRECTION 

MATA(4,4)  =  l.ODO 
MATA(4,8)  =  RBG1( 3) 
MATA(4,9)  =  -RBG1C2) 
MATB( 4)  =  AXO  -  MICO 

Y  DIRECTION 

MATA(5 ,5)  =  l.ODO 
MATA( 5,7)  =  -RBG1(3) 
MATA(5,9)  =  RBGl(l) 
MATE ( 5 )  =  AYO  >  MJCO 

Z  DIRECTION 

MATA(6 ,6)  =  l.ODO 
MATA( 6,7)  =  RBGlf  2) 
MATA( 6,8)  =  -RBGl(l) 


1 05 


MATB ( 6 )  =  AZO  -  MKCO 

SUM  OF  MOMENTS  EQUATIONS 


X  DIRECTION 

MATA( 7,2) 

= 

RBG1( 3) 

MATA( 7,3) 

= 

-RBG1(2) 

MATA( 7,7) 

= 

-IXXT(l) 

MATA( 7,8) 

= 

IXYT( 1) 

MATA( 7,9) 

= 

IXZT(l) 

MATA( 7,11) 

= 

-RAG1(3) 

MATA( 7,12) 

= 

RAG1(2) 

MATB( 7 ) 

= 

T1X  -  TOX 

Y  DIRECTION 

MATA( 8,1) 

= 

-RBG1(3) 

MATA( 8,3) 

= 

RBGl(l) 

MATA( 8,7) 

= 

IXYT( 1 ) 

MATA( 8,8) 

= 

-IYYT( 1) 

MATA( 8,9) 

IYZT(l) 

MATA( 8,10) 

RAG1( 3) 

MATA( 8,12) 

= 

-RAG1( 1) 

MATB(8) 

T1Y  -  TOY 

Z  DIRECTION 

MATA( 9,1) 

= 

RBG1(2) 

MATA( 9,2) 

= 

-RBGl(l) 

MATA( 9,7) 

= 

IXZT(l) 

MATA( 9,8) 

IYZT(l) 

MATA( 9,9) 

-IZZT(l) 

MATA( 9,10) 

= 

-RAG1(2) 

MATA( 9,11) 

= 

RAGl(l) 

MATB ( 9 ) 

T1Z  -  TOZ 

LINK  TWO 
SUM  OF  FORCES 
X  DIRECTION 


460 

MATA( 10,10) 

= 

1.  ODO 

MATA( 10,13) 

=r 

M2 

MATA( 10,19) 

r= 

-1.  ODO 

MATB( 10) 

= 

0.  ODO 

* 

Y  DIRECTION 

MATAC 11,11) 

= 

1.  ODO 

MATA( 11,14) 

= 

M2 

MATA( 11,20) 

s 

-1.  ODO 

MATB(ll) 

= 

0.  ODO 

* 

Z  DIRECTION 

MATA( 12,12) 

1.  ODO 

MATA( 12,15) 

= 

M2 

MATA( 12,21) 

= 

-1.  ODO 

MATB( 12) 

= 

-WG2 

■j k 

EQUATIONS  AT  . 

JOINT  ONE 

* 

X  DIRECTION 

MATAC 13,4) 

= 

-1.  ODO 

MATAC 13,8) 

= 

-RAG1C3) 

MATAC 13,9) 

RAG1C2) 

MATAC 13, 13) 

1.  ODO 

MATAC 13, 17) 

= 

RBG2(3) 

MATAC 13,18) 

= 

-RBG2(2) 

MATB( 13) 

= 

MICl  -  MIC2 

* 

Y  DIRECTION 

MATA( 14 , S) 

-1. 0D0 

MATAC 14,7) 

= 

RAG1C3) 

MATAC 14,9) 

-RAGl(l) 

MATA( 14,14) 

= 

1.  ODO 

MATA( 14,16) 

= 

-RBG2(3) 

MATA( 14,18) 

s ; 

RBG2C1) 

MATBC14) 

= 

MJC1  -  MJC2 

* 

Z  DIRECTION 

MATA( 15,6) 

-1.  ODO 

MATA( 15,7) 

= 

-RAG1C2) 

MATA( 15,8) 

RAG1C1) 

MATA( 15,15) 

s: 

1.  ODO 

MATAC 15,16) 

s 

RBG2C2) 

MATAC 15,17) 

= 

-RBG2C 1) 

MATB( 15 ) 

- 

MKC1  -  MKC2 

* 

SUM  OF  MOMENTS  EQUATIONS 

X  DIRECTION 

MATA( 16, 11) 

= 

RBG2C  3) 

MATAC 16, 12) 

ss 

-RBG2C2) 

MATAC 16, 16) 

= 

-IXXTC2) 

MATA( 16, 17) 

2= 

IXYTC2) 

MATAC 16,18) 

=: 

1XZTC2) 

MATAC 16,20) 

-RAG2C3) 

MATAC 16,21) 

RAG2C2) 

MATBC16) 

T2X  -  T1X 

* 

Y  DIRECTION 

MATAC 17, 10) 

= 

-RBG2(3) 

MATAC 17, 12) 

= 

RBG2(1) 

MATAC 17, 16) 

= 

IXYTC2) 

MATAC 17, 17) 

= 

-IYYTC2) 

MATAC 17,18) 

IYZTC2) 

MATAC 17, 19) 

RAG2C3) 

MATAC 17,21) 

= 

-RAG2C1) 

MATBC17) 

= 

T2Y  -  T1Y 

Vr 

Z  DIRECTION 

MATAC 18, 10) 

sr 

RBG2C2) 

MATAC 18,11) 

= 

-RBG2C1) 

MATAC 18,16) 

= 

IXZTC2) 

MATAC 18, 17) 

= 

IYZTC2) 

MATAC 18,18) 

2= 

-IZZTC2) 

MATAC 18,19) 

= 

-RAG2C2) 

MATAC 18,20) 

= 

RAG2C1) 

MATBC18) 

2 

T2Z  -  T1Z 

** 

LINK  THREE 

* 

SUM  OF  FORCES 

* 

X  DIRECTION 

530 

MATAC 19, 19) 

2 

1.  ODO 

MATAC 19, 22) 

2 

M3 

MATBC19) 

= 

0.  ODO 

* 

Y  DIRECTION 

MATAC 20, 20) 

2 

1.  ODO 

MATAC 20, 23) 

s 

M3 
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MATB(20)  =  0.  ODO 

*  Z  DIRECTION 

MATA(21 ,21)  =  1.  ODO 

MATA( 21,24)  =  M3 
MATB( 21)  =  -WG3 

*  EQUATIONS  AT  JOINT  TWO 

*  X  DIRECTION 

MATA( 22,13)  =  -l.ODO 
MATA(22, 17)  =  -RAG2(3) 

MATA( 22,18)  =  RAG2(2) 

MATA( 22,22)  =  l.ODO 
MATA( 22,26)  =  R3G3(3) 

MATA( 22,27)  =  -RBG3(2) 

MATB(22)  =  MIC3  -  MIC4 

*  Y  DIRECTION 

MATA( 23 , 14)  =  -l.ODO 
MATA( 23,16)  =  RAG2(3) 

MATA( 23 , 18)  =  -RAG2(1) 

MATA( 23,23)  =  l.ODO 
MATA( 23,25)  =  -RBG3(3) 

MATA( 23,27)  =  RBG3(1) 

MATB(23)  =  MJC3  -  MJC4 

*  Z  DIRECTION 

MATA( 24,15)  =  -1.  ODO 
MATA(24, 16)  =  -RAG2(2) 

MATA( 24,17)  =  RAG2(1) 

MATA(  24 , 24)  =  l.ODO 
MATA( 24,25)  =  RBG3(2) 

MATA( 24 , 26)  =  -RBG3(1) 

MATBC24)  =  MKC3  -  MKC4 

*  SUM  OF  MOMENTS  EQUATIONS 

*  X  DIRECTION 

MATA( 25 , 20)  =  RBG3(3) 

MATA( 25,21)  =  -RBG3(2) 

MATA( 25,25)  =  -IXXT(3) 

MATA( 25 , 26)  =  IXYT(3) 

MATA( 25,27)  =  IXZT(3) 

MATBC25)  =  -T2X 

*  Y  DIRECTION 

MATA( 26,19)  =  -RBG3(3) 

MATA(26,21)  =  RBG3(1) 

MATA( 26,25)  =  IXYT(3) 

MATA( 26,26)  =  -IYYT(3) 

MATA( 26,27)  =  IYZT(3) 

MATB( 26)  =  -T2Y 

*  Z  DIRECTION 

MATA( 27,19)  =  RBG3(2) 

MATA( 27,20)  =  -RBG3(1) 

MATA(27 ,25)  =  IXZT(3) 

MATA( 27 , 26)  =  IYZT(3) 

MATA( 27 ,27 )  =  -IZZT(3) 

MATB( 27)  =  -T2Z 

*****  FIRST  LINK  IS  CONSTRAINED  NOT  TO  ROTATE  ***** 
DO  600  I  =4,9 
DO  595  J  =  1,27 


I  OS 


MATA(I,J)  =  0.  0D0 
MATA( J , I )  =  0.  ODO 
595  CONTINUE 

MATB(I)  =  0.  ODO 
600  CONTINUE 

MATA(4 ,4)  =  1.  ODO 
MATA(5 ,5)  =  1.  ODO 
MATA(6,6)  =  1.  ODO 
MATA( 7,7)  =  1. ODO 
MATA(8,8)  =  1.  ODO 
MATA( 9,9)  =  1.  ODO 
DO  602  I  =  13,17 
DO  597  J  =  1,27 
MATA( I , J)  =  0. ODO 
MATA(J,I)  =  0. ODO 
597  CONTINUE 

MATB(I)  =  0. ODO 
602  CONTINUE 

MATA( 13,13)  =  1. ODO 
MATA( 14,14)  =  1. ODO 
MATA( 15,15)  =  1. ODO 
MATA( 16,16)  =  1. ODO 
MATA( 17,17)  =  1. ODO 


605 

DO  610  J  =  1, 

27 

MATA( 18, J) 

= 

0.  ODO 

MATA( 27 ,J) 

= 

0.  ODO 

610 

CONTINUE 

MATA( 18,9) 

-1. ODO 

MATA( 18,18) 

= 

1.  ODO 

MATB( 18) 

= 

0.  ODO 

MATA( 27,9) 

= 

-1.  ODO 

MATA(27 ,27) 

= 

1.  ODO 

MATB( 27 ) 

= 

0.  ODO 

*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

620  CALL  LEQT2F( MATA ,M,N, IA,MATB , IDGT.WKAREA, IER) 

IF  (IER. EQ. 0)  THEN 

GOTO  640 

ELSE 

WRITE  (*,624)  IER 
624  FORMAT  (15) 

DO  635  1=1,  27 

WRITE  (*,627)  I 
627  FORMAT  (17) 

DO  631  J  =  1,  27,  3 

WRITE  (*,630)  J ,MATA( I , J) , J+l ,MATA( I ,  J+l) , J+2 ,MATA( I , J+2) 

630  FORMAT  ( 15 ,F13. 5 , 15 ,F13. 5 , 15 ,F13. 5) 

631  CONTINUE 

WRITE  (*,633)  I ,MATB( I ) 

633  FORMAT  (13, F15. 5) 

635  CONTINUE 

CALL  ENDJOB 
END  IF 
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***  FIN’D  LCOGX , LCOGY , LCOG2 , THETA  VALUES ,WX,WY,WZ 
***  MODIFIED  BY  R. M. VERBOS 


*  JOINT  ZERO 

640  FXO  =  MATB(l) 

FYO  =  MATB ( 2 ) 

FZO  =  MATB( 3) 

*  LINK  ONE 

*  SINCE  LINK1  "S  CONSTRAIN  TO  ROTATE  AROUND  THE  Z  ONLY 

AX1  =  0.  ODO 
AY1  =  0.  ODO 
AZ1  =  0. 0 DO 


*660 

AXl 

= 

MATBC4) 

* 

VELX1 

= 

INTGRLCO.  , AXl ) 

* 

LC0GX1 

INTGRLCX1 jVELXl) 

Vc 

LC0GXC1) 

= 

LCOGX 1 

Vc 

AY1 

= 

MATB( 5) 

Vc 

VELY1 

= 

INTGRLCO.  , AY1 ) 

* 

LCOGY 1 

= 

INTGRLC Y1 , VELY1) 

Vc 

LCOGYC 1) 

= 

LCOGY 1 

Vc 

AZ1 

= 

MATBC  6) 

Vc 

VELZ1 

INTGRLCO.  , AZ1 ) 

Vr 

LCOGZ 1 

= 

INTGRLC Zl.VELZl) 

Vr 

LCOGZC 1) 

= 

LCOGZ 1 

WD1X  =  MATB( 7 ) 

W1X  =  INTGRLCO. ,WD1X) 

VDX(l)  =  WD1X 

W 1 ( 1 )  =  W1X 

WD1Y  =  MATB ( 8 ) 

W1Y  =  INTGRLCO. ,WD1Y) 

WDY(l)  =  WD1Y 

VU  2)  =  W1Y 

WD1Z  =  MATB(9) 

W1Z  =  INTGRLCO. ,WD1Z) 

WDZC1)  =  WD1Z 

W i ( 3 )  =W1Z 

***  ADDED  BY  R. M. VERBOS 

685  THZ  =  INTGRLCO. ,W1Z) 

THZANG  =  THZ  *  RADEG 

COSTHZ  =  DCOS(THZ) 

SINTHZ  =  DS IN( THZ) 

*  IF  THE  1ST  LINK  IS  CONSTRAIN  TO  ROTATE  IN  THE  Z  DIRECTION  ONLY 

*  THE  DIRECTION  COSINE  AND  DIRECTION  COSINE  ANGLES  ARE  CONSTANT 

*  AND  DO  NOT  NEED  TO  BE  CALCULATED 

* 

*  CALC  DIRECTION  .L  COSINES  FOR  LINK  ONE 

* 

*695  LNC0G1  =  DSQRT  C  LC0GX1*LC0GX1  +  LC0GY1*LC0GY1  +.  .  . 

*  LCOGZ 1*LC0GZ 1  ) 

*700  DRCSX(l)  =  LCOGX 1  /  LNC0G1 

*  DRCSY(l)  =  LCOGY 1  /  LNC0G1 

*  DRCSZC1)  =  LCOGZ 1  /  LNC0G1 

*  AMP  =  DSQRTCDRCSXC 1)*DRCSX( 1)+DRCSY( 1)*DRCSY(  1)+.  .  . 

*  DRCSZ( 1)*DRCSZ( 1) ) 


110 


* 

DRCSX(l) 

=  DRCSX( 1)/AMP 

it 

DRCSY(l) 

=  DRCSY(1)/AMP 

* 

DRCSZ(l) 

=  DRCSZ( 1)/AMP 

*720 

DRCANX(l)  =  DACOS(DRCSX(l))  *  RADEG 

it 

DRCANY(l)  =  DACOS(DRCSY( 1) )  *  RADEG 

it 

it 

DRCANZ(l)  =  DAC0S(DRCSZ(1))  *  RADEG 

** 

JOINT  LOCATION 

*730 

JX1  = 

LINKL1  *  DRCSX(l) 

* 

JY1  = 

LINKL1  *  DRCSY(l) 

* 

JZ1  = 

LINKL1  *  DRCSZC1) 

* 

JOINT  ONE 

735 

FX1  =  MATB(IO) 

FY1  =  MATB(ll) 

FZ1  =  MATB( 12) 

AX 2  =  0.  ODO 

AY2  =  0.  ODO 

AZ2  =  0.  ODO 

itit 

LINK  TWO 

740 

AX  2 

=  MATB(13) 

it 

VELX2 

=  INTGRL( 0.  ,AX2) 

a 

LC0GX2 

=  INTGRL(X2 , VELX2) 

it 

LC0GX(2) 

=  LC0GX2 

it 

AY2 

=  MATB( 14) 

* 

VELY2 

=  INTGRL( 0.  , AY2) 

* 

LC0GY2 

=  INTGRL( Y2 ,VELY2) 

it 

LCOGY( 2) 

=  LC0GY2 

it 

AZ2 

=  MATB(15) 

it 

VELZ2 

=  INTGRL( 0.  , AZ2) 

it 

LC0GZ2 

=  INTGRL( Z2 , VELZ2) 

it 

LCOGZ( 2) 

=  LC0GZ2 

WD2X 

=  MATB( 16) 

W2X 

=  INTGRL( 0. ,WD2X) 

WDX( 2) 

=  WD2X 

W2(l) 

=  W2X 

WD2Y 

=  MATB (17) 

W2Y 

=  INTGRL( 0.  ,WD2Y) 

WDY(  2 ) 

=  WD2Y 

W2(  2 ) 

=  W2Y 

WD2Z 

=  MATB( 18) 

W2Z 

=  INTGRLCO.  ,WD2Z) 

WDZ( 2) 

=  WD2Z 

W2(  3) 

=  W2Z 

* 

GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 

* 

DRCSX(2) 

=  (LC0GX2  -  JX1)  /  LNC0G2 

it 

DRCSY(2) 

=  (LC0GY2  -  JYI )  /  LNC0G2 

it 

DRCSZ(2) 

=  (LC0GZ2  -  JZ1)  /  LNC0G2 

*90 

AMP  =  DSQRT(DRCSX( 2)*DRCSX( 2)+DRCSY( 2)*DRCSY(2)+. 

* 

DRCSZ( 2)*DRCSZ( 2) ) 

it 

DRCSXC2) 

=  DRCSX( 2) /AMP 

it 

DRCSYC2) 

=  DRCSY( 2)/AMP 

it 

DRCSZ(2) 

=  DRCSZ(2)/AMP 

Ill 


* 

* 

Vr 


DRCANX( 2 )  =  DACOS(DRCSX( 2) )  *  RADEG 
DRCANY( 2)  =  DACOS( DRCSY( 2 ) )  *  RADEG 
DRCANZ( 2)  =  DACOS(DRCSZ( 2) )  *  RADEG 


*  JOINT  LOCATION 

800  JX2  =  JX1  +  LINKL2  *  DRCSX(2) 

JY2  =  JY1  +  LINKL2  *  DRCSY(2) 
JZ2  =  JZ1  +  LINKL2  *  DRCSZ(2) 

*  JOINT  TWO 

805  FX2  =  MATB( 19) 

FY2  =  MATB( 20) 

FZ2  =  MATB(21) 

**  LINK  THREE 


AX3 

= 

MATB( 22) 

VELX3 

= 

INTGRL(0.  , AX3) 

LCOGX3 

= 

INTGRLf  X3 , VELX3 ) 

LC0GXC3) 

= 

LCOGX3 

AY3 

s 

MATB( 23) 

VELY3 

= 

INTGRL(V3IC,AY3) 

LCOGY3 

I NTGRL( Y3 , VELY3 ) 

LC0GY(3) 

= 

LCOGY3 

AZ3 

= 

MATB(24) 

VELZ3 

= 

INTGRL(0.  , AZ3) 

LCOGZ3 

= 

INTGRL( Z3 , VELZ3) 

LCOGZ(3) 

= 

LCOGZ3 

WD3X 

MATB(25) 

W3X 

= 

INTGRL(W3IC,WD3X) 

WDX( 3) 

= 

WD3X 

V>3(  1) 

= 

W3X 

WD3Y 

T- 

MATB( 26) 

W3Y 

INTGRL( 0. ,WD3Y) 

WDY( 3) 

= 

WD3Y 

W3(  2) 

= 

W3Y 

WD3Z 

MATBC27) 

W3Z 

= 

INTGRLC  0.  ,WD3Z) 

WDZ( 3) 

=r 

WD3Z 

W3(  3) 

= 

W3Z 

*  CALC  DIRECTIONAL  COSINES  FOR  LINK  THREE 

845  DRCSXC3)  =  (LCOGX3  -  JX2)  /  LNCOG3 

DRCSY( 3)  =  ( LCOGY3  -  JY2)  /  LNCOG3 
DRCSZ( 3)  =  (LCOGZ3  -  JZ2)  /  LNCOG3 
865  AMP  =  DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+.  .  . 

DRCSZ( 3)*DRCSZ( 3) ) 

DRCSX( 3)  =  DRCSX( 3)/AMP 

DRCSY( 3)  =  DRCSY( 3)/AMP 

DRCSZ( 3)  =  DRCSZ( 3)/AMP 

DRCANX(3)  =  DAC0S(DRCSX(3))  *  RADEG 

DRCANY( 3)  =  DACOS(DRCSY( 3) )  *  RADEG 

DRCANZ( 3)  =  DACOS(DRCSZ( 3) )  *  RADEG 

*  TIP  LOCATION 

875  TIPX  =  JX2  +  LINKL3  *  DRCSX(3) 

TIPY  =  JY2  +  LINKL3  *  DRCSY(3) 
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TIP2  =  JZ2  +  LINKL3  *  DRCSZ(3) 


*  FIND  THE  ANGLE  BETWEEN  THE  LIMKS 

880  ANG23X  =  DRCANX(2)  -  DRCANX(3) 

ANG23Y  =  DRCANY( 2)  -  DRCANY( 3) 

ANG23Z  =  DRCANZ( 2)  -  DRCANZ(3) 

ANG12X  =  DRCANX(l)  -  DRCANX(2) 

ANG12Y  =  DRCANY(l)  -  DRCANY( 2) 

ANG12Z  =  DRCANZ(l)  -  DRCANZ(2) 

DYNAMIC 

NOSORT 

*****  INPUT  TORQUE  AT  JOINTS 

T2FNC  =  (13.5  *  SIN  (PI  /  (30.0)  *  TIME))  *  RCNFAC 
T2  =  T2FNC 
T2X  =  T2 

END 

STOP 


FORTRAN 

*  SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPROD( VECTA , VECTB , MI , M J ,  MK) 

940  IMPLICIT  REAL* 8  (A-Z) 

DIMENSION  VECTA( 3 ) , VECTB ( 3 ) 

MI  =  VECTA( 2)  *  VECTB( 3)  -  VECTA(3)  *  VECTB(2) 

MJ  =  VECTA(3)  *  VECTB(l)  -  VECTA(l)  *  VECTB(3) 

MK  =  VECTA(l)  *  VECTB( 2)  -  VECTA(2)  *  VECTB(l) 

RETURN- 
END 
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APPENDIX  E 


BASIC  OPERATING  INSTRUCTIONS  FOR  THE  NAVAL 
POSTGRADUATE  SCHOOL  RIGID  MANIPULATOR  TEST  BED 

A.  SYSTEM  FAMILIARIZATION 

Before  operating  the  manipulator  test  bed,  you  should  ensure  you 
are  familiar  with  its  components  and  basic  procedures  of  operation.  If 
you  are  already  familiar  with  the  components  of  the  manipulator  and 
their  set-up.  you  will  need  to  review  this  section  to  operate  the 
hardware. 

The  test  bed  consists  of  four  major  sub-systems:  the  Neptune  II 
hydraulically  operated  manipulator;  an  IBM  PC -AT  used  to  control  the 
manipulator  and  gather  data  (at  the  present  time);  an  IBM  PC -XT  (not 
in  the  system  at  present);  and  an  Electronic  Control  Panel  used  in 
switching,  data  gathering,  and  interfacing. 

1.  The  Manipulator 

The  manipulator  (Figure  E-l),  being  a  hydraulic  mechanism, 
is  subject  to  leakage.  It  is  advisable  to  wear  old  clothing  while  in  the 
lab.  The  operating  pressure  of  the  system  is  around  100  psi  and  a 
minor  leak  may  spray  anywhere  in  the  lab. 

The  source  of  the  hydraulic  fluid  is  the  pump  cabinet  against 
the  forward  side  of  the  wall  in  front  of  the  computers.  This  cabinet 
contains  a  pump  which  is  operated  from  the  ECP,  a  reservoir  which  is 
underneath  the  pump,  and  an  accumulator  that  is  alongside  of  the 
pump  (Figure  E-2). 
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Figure  E-l.  Manipulator 
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Figure  E-2.  Pump  Cabinet 
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The  reservoir  has  a  fill  hole  on  the  top.  It  is  necessary  to 
check  this  before  operating  the  system  and  ensure  that  the  level  is 
around  half  when  the  system  is  not  in  operation.  You  will  also  notice  a 
pressure  gage  which  reads  the  pressure  in  the  accumulator.  This 
pressure  should  be  between  80  and  100  psi  and  may  be  increased  by 
pressurizing  the  bladder  in  the  accumulator  to  a  higher  pressure. 

If  you  look  closely  at  the  accumulator,  you  will  notice  a 
solenoid  valve  at  the  outlet.  This  valve  has  been  installed  because  of 
previous  problems  and  will  cut  off  all  hydraulic  flow  to  the  manipulator 
when  it  is  shut  from  the  ECP. 

Underneath  the  manipulator  itself  is  a  set  of  Electronic 
Printed  Circuit  Boards  (EPCB).  These  boards  control  the  opening  and 
shutting  of  the  solenoid  values  to  the  joint  pistons  (see  Ref.  9  for  more 
information).  The  important  thing  to  know  about  these  boards  is  that 
you  should  avoid  getting  hydraulic  fluid  on  them. 

2.  The  Computers 

At  present,  the  IBM-AT  is  the  only  computer  hooked  up  in 
the  system.  It  is  easy  to  follow  because  it  has  been  set  up  with  a  menu- 
based  operating  system.  The  power  to  the  AT  is  under  the  right  side  of 
the  computer  stand.  The  first  thing  that  will  happen  when  you  turn 
the  power  on  is  that  the  AT  will  go  thru  a  cold-start  boot-up,  which 
may  take  a  minute  or  two.  You  will  then  notice  a  listing  of  subdirecto¬ 
ries  on  the  screen.  Most  of  these  subdirectories  are  of  little  use 
because  the  files  are  proprietary  in  nature  and  the  author  has  long 
since  transferred  away  from  NPS,  but  feel  free  to  skim  through  them. 
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For  the  initial  running  of  the  manipulator,  you  will  want  to 
select  the  BATCHES  subdirectory.  Once  you  are  in  this  subdirectory, 
you  will  select  the  NEPTROLA  file  to  run  the  manipulator.  This  file 
contains  the  control  program  for  operating  the  manipulator  in  the 
solenoid  mode.  The  program  is  a  useful  starting  point  for  examining 
the  range  of  the  arm  motion. 

NEPTROLA  is  a  menu-driven  set-up  that  asks  various  ques¬ 
tions  to  get  the  system  in  the  proper  operating  configuration.  The 
actual  operation  will  be  discussed  in  the  operation  section  of  this 
appendix. 

3.  Electronic  Control  Panel  (ECP) 

The  ECP  is  located  between  the  IBM-AT  and  the  IBM-XT  and 
is  the  heart  of  the  control  of  the  manipulator.  The  ECP  contains  sev¬ 
eral  interface  boards  which  should  not  be  tampered  with  without 
talking  to  Tom  Cristian  of  the  Mechanical  Engineering  technical  staff. 
The  front  of  the  panel  is  divided  into  four  sections  (Figure  E-3).  Start¬ 
ing  at  the  top,  the  first  sub-panel  consists  of  two  sets  of  electrical 
jacks.  The  first  six  from  the  left  are  connected  to  the  position  data  of 
the  interface  boards  and  will  provide  joint  0  thru  5  position  output. 
The  last  two  jacks  are  for  servo  operation  which  is  no  longer  con¬ 
nected  (more  information  on  servo  operation  is  available  in  Ref.  13). 
The  next  panel  down  was  for  control  of  the  servovalves;  again,  more 
information  is  available  in  Reference  13.  The  next  panel  contains  a 
power  switch  which  selects  power  to  the  panel  and  electrical  jacks  to 


BACK  OF  PRINTED  CIRCUIT  BOARD 
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monitor  the  pressure  transducer  output  for  torque  calculations.  Pres¬ 
sure  transducers  have  been  installed  on  joints  1  and  2.  The  electrical 
Jacks  are  wired  so  that  PI  and  P2  are  connected  to  joint  1  transduc¬ 
ers.  P3  and  P4  are  connected  to  joint  2  transducers,  and  P5  and  P6 
are  available  for  further  connections. 

The  bottom  panel  is  the  power  panel  and  consists  of  seven 
switches  and  three  electrical  jacks.  The  switches,  from  left  to  right, 
are  as  follows: 

•  Electronic  Control  Power,  which  supplies  the  electricity  to  the 
interface  board  at  the  back  of  the  panel. 

•  Solenoid  Current  Switch,  which  must  be  “ON"  to  operate  the 
manipulator  in  the  solenoid  mode. 

•  Servo  Master  Switches  (3),  which  are  three-position  switches: 

-  (DOWN)  is  for  selecting  servovalve  control  from  the  AT 

-  (MIDDLE)  is  for  operating  in  the  solenoid  mode 

-  (UP)  is  for  for  operating  the  servovalves  from  an  auxiliary  signal 
which  is  connected  to  the  three  electrical  jacks  above  the 
switches. 

•  Extra  switch. 

•  (Rightmost)  Pump  Power/ Solenoid  Valve  Control  Switch.  This  is  a 
three-position  switch: 

-  (UP)  the  pump  is  in  the  run  mode  and  controlled  by  pressure 

-  (MIDDLE)  the  pump  is  off  and  the  solenoid  valve  is  open 

-  (DOWN)  the  pump  is  off  and  the  solenoid  valve  has  power  to  it 
and  is  shut  (this  is  an  emergency  cut-off  position). 
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B.  OPERATING  THE  ARM 


1.  The  Solenoid  Mode 

Following  is  a  step-by-step  procedure  for  operating  the  Nep¬ 
tune  II  manipulator  in  the  solenoid  mode: 

STEP  1 

Check  the  accumulator  to  ensure  that  there  is  sufficient  fluid  to 
operate  the  manipulator.  If  the  level  is  low.  add  a  50/50  mix  of 
Hydro  Lube  and  water  until  the  level  is  between  one-half  and 
three-quarters  (DO  NOT  FILL). 

STEP  2 

Check  the  accumulator  pressure  to  ensure  it  is  between  80  and 
100  psi.  If  the  pressure  is  low  or  high,  bleed  or  add  air  through 
the  hose  connection  as  necessary. 

STEP  3 

From  the  ECP  turn  the  following  switches  to  the  position  referred 
to: 

-  Electronic  Power  Switch  ON  (UP) 

-  Solenoid  Current  Switch  ON  (UP) 

-  Servo  Master  Switches  OFF  (MIDDLE) 

-  Pump  Power  Switch  ON  (UP) 

STEP  4 

Now  turn  the  IBM-AT  computer  on. 

STEP  5 

Select  the  BATCHES  subdirectory. 

STEP  6 

Select  the  NEPTROLA  program  and  answer  “yes’*  (Y)  to  the  ques¬ 
tion  whether  you  want  to  operate  without  ADC.  This  will  allow  you 
to  operate  from  the  keyboard.  The  next  question  will  give  several 
options— pick  the  keyboard  option.  Follow  the  given  computer 
menu  and  vary  the  joint  positions  as  desired. 
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STEP  7 

To  leave  the  NEPTROLA  program,  hit  ESC,  then  hit  the  control 
and  break  key  at  the  same  time.  After  this,  type  “SYSTEM"  and 
you  will  be  returned  to  the  BATCHES  subdirectory  menu. 

2.  Servovalve  Operation 

At  present,  the  servo  valves  have  been  removed  from  the  sys¬ 
tem  but  the  software  and  wiring  are  still  available.  When  the  servo¬ 
valves  are  reinstalled,  refer  to  [Ref.  13)  for  operating  procedures. 
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